Commits

Trammell Hudson committed 39495fe Draft

Control loop works! Decent gains for joint #5

Comments (0)

Files changed (1)

 	const uint8_t i = 1;
 	servo_t * const servo = &servos[i];
 
-	const int p_gain = 16;
-	const int d_gain = 16;
+	const int p_gain =  8;
+	const int d_gain = 64;
 
 	int16_t position = servo_position(i);
 
 	int16_t error = servo->command - position;
+	if (error < -1000)
+		error = -1000;
+	else
+	if (error > +1000)
+		error = +1000;
+
 	int16_t v_instant = position - servo->last_position;;
 	const int SCALE = 7;
 	int16_t velocity = servo->velocity = (servo->velocity * SCALE + v_instant) / (SCALE+1);
 	servo->last_position = position;
 
-	int16_t output = (error * p_gain) / 256 + (velocity * d_gain) / 256;
+	int16_t output = (error * p_gain) / 256 - (velocity * d_gain) / 256;
 ;
 	if (output < -127)
 		output = -127;
 		| 0 << WGM02
 		| 1 << CS02 // select Clk/1024
 		| 0 << CS01
-		| 1 << CS00
+		| 0 << CS00
 		;
 
 	OCR0A = 244;