Punkt 1 ist schon mal: Config wird nur 1x gemacht. Also vor das Do.
Punkt 2: Waitms 20000 sind keine 2 Sekunden.
Punkt 3: Zum Abbremsen der Motoren müsstest du PWM1A schrittweise zum jeweiligen Wert rauf/runterzählen (je nach dem, ob die Motoren bei 0 oder 65535 stehen bleiben).
Also ne Zählschleife, da ne Variable runterzählen und die dann auf PWM1A zuweisen.

Was hast du dann da noch mit Timer1 = 1 vor?