schau dir das unten an für die pwm.
mit deiner hast du nur ca 15khz, beim rp6 muss die pwm ca mit 19khz laufen, ansonsten brennen die motortreiber durch.

weiterhin hilfst du den rp6 über den jordan, weil du kein sanftes anfahren machst und der wechsel der richtung zu schnell geht.

wenn du den aufbau und die baugruppenleistungen vom rp6 nicht kennst, lass lieber die finger weg.

Code:
// Initialize Timer1 - PWM:
	// PWM, phase correct with ICR1 as top value.
	TCCR1A = (0 << WGM10) | (1 << WGM11) | (1 << COM1A1) | (1 << COM1B1);
	TCCR1B =  (1 << WGM13) | (0 << WGM12) | (1 << CS10);
	ICR1 = 210; // Phase corret PWM top value - 210 results in 
				// about 19 kHz PWM.
				// ICR1 is the maximum (=100% duty cycle) PWM value!
				// This means that the PWM resolution is a bit lower, but
				// if the frequency is lower than 19 kHz you may hear very
				// annoying high pitch noises from the motors!
				// 19 kHz is a bit over the maximum frequency most people can
				// hear!
				// 
				// ATTENTION: Max PWM value is 210 and NOT 255 !!!