Commits

Trammell Hudson  committed 31f7df9 Draft

Timer0 64 Hz loop in place

  • Participants
  • Parent commits 9a99c95

Comments (0)

Files changed (1)

 
 
 /** Position counters for each axis */
-static int16_t positions[6];
-static int16_t commands[6];
+typedef struct
+{
+	int16_t position; // must access with interrupts locked
+	int16_t last_position;
+	int8_t velocity;
+	int16_t command;
+} servo_t;
+
+static servo_t servos[6];
+
+static int16_t
+servo_position(
+	uint8_t i
+)
+{
+	cli();
+	int16_t p = servos[i].position;
+	sei();
+	return p;
+}
+
 
 static inline void
 quad_decode(
 	const uint8_t y = in(y_port) ? 1 : 0;
 	const uint8_t dir = x ^ y;
 
-	positions[axis] += dir ? 1 : -1;
+	servos[axis].position += dir ? 1 : -1;
 }
 
 
 	int16_t command
 )
 {
-	commands[i] = command;
+	servos[i].command = command;
 }
 
 
 static void
 position_loop(void)
 {
-	if (!output_enabled)
-		return;
+	const uint8_t i = 1;
+	servo_t * const servo = &servos[i];
 
-	const int i = 1;
-	const int gain = 16;
+	const int p_gain = 16;
+	const int d_gain = 16;
 
-	int16_t error = commands[i] - positions[i];
-/*
-	if (-128 < error)
-		error = -128;
-	else
-	if (+127 > error)
-		error = 127;
-*/
+	int16_t position = servo_position(i);
 
-	int16_t output = (error * gain) / 256;
-	if (output < -128)
-		output = -128;
+	int16_t error = servo->command - position;
+	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;
+;
+	if (output < -127)
+		output = -127;
 	else
 	if (output > 127)
 		output = 127;
 		
-	output_drive(i, output);
+	if (output_enabled)
+		output_drive(i, output);
+}
+
+
+static void
+position_output(void)
+{
+	char buf[80];
+	uint8_t off = 0;
+
+	off += print_position(buf+off, servo_position(1));
+	buf[off++] = ' ';
+	off += print_position(buf+off, servos[1].velocity);
+	buf[off++] = ' ';
+	off += print_position(buf+off, OCR2A);
+	buf[off++] = ' ';
+
+#if 0
+	off += print_position(buf+off, position_read(0));
+	buf[off++] = ' ';
+	off += print_position(buf+off, position_read(1));
+	buf[off++] = ' ';
+	off += print_position(buf+off, position_read(2));
+#if 0 // don't have the other three motors yet
+	buf[off++] = ' ';
+	off += print_position(buf+off, position_read(3));
+	buf[off++] = ' ';
+	off += print_position(buf+off, position_read(4));
+	buf[off++] = ' ';
+	off += print_position(buf+off, position_read(5));
+#endif
+#endif
+
+	buf[off++] = '\r';
+	buf[off++] = '\n';
+
+	usb_serial_write(buf, off);
 }
 
 
 	ddr(LED, 1);
 	out(LED, 1);
 
+	// Timer 0 is used for a 64 Hz control loop timer.
+	// Clk/1024 == 15.625 KHz, count up to 244
+	// CTC mode resets the counter when it hits the top
+	TCCR0A = 0
+		| 1 << WGM01 // select CTC
+		| 0 << WGM00
+		;
+
+	TCCR0B = 0
+		| 0 << WGM02
+		| 1 << CS02 // select Clk/1024
+		| 0 << CS01
+		| 1 << CS00
+		;
+
+	OCR0A = 244;
+	sbi(TIFR0, OCF0A); // reset the overflow bit
+
 	// Configure OC1x in fast-PWM mode, 10-bit
 	// REF+ is OC1A
 	sbi(TCCR1B, WGM12);
 	send_str(PSTR("servo driver\r\n"));
 	uint16_t val = 0;
 	uint8_t sign = 1;
+	uint8_t phase = 0;
 
 	while (1)
 	{
+		if (bit_is_set(TIFR0, OCF0A))
+		{
+			sbi(TIFR0, OCF0A); // reset the bit
+			position_loop();
+			if ((phase++ & 0xF) == 0)
+				position_output();
+		}
+
 		int c = usb_serial_getchar();
 		if (c == -1)
 		{
-			position_loop();
-
-			if (bit_is_clear(ADCSRA, ADIF))
-				continue;
-
-			// current measurement is ready
-			uint16_t val = ADC;
-			sbi(ADCSRA, ADIF);
-			sbi(ADCSRA, ADSC);
-			(void) val;
-
-			char buf[80];
-			uint8_t off = 0;
-
-			off += print_position(buf+off, positions[0]);
-			buf[off++] = ' ';
-			off += print_position(buf+off, positions[1]);
-			buf[off++] = ' ';
-			off += print_position(buf+off, positions[2]);
-#if 0 // don't have the other three motors yet
-			buf[off++] = ' ';
-			off += print_position(buf+off, positions[3]);
-			buf[off++] = ' ';
-			off += print_position(buf+off, positions[4]);
-			buf[off++] = ' ';
-			off += print_position(buf+off, positions[5]);
-#endif
-
-			buf[off++] = '\r';
-			buf[off++] = '\n';
-
-			usb_serial_write(buf, off);
 			continue;
 		}
 
 			// this avoids any jumps
 			for (int i = 0 ;i < 6 ; i++)
 			{
-				position_command(i, positions[i]);
+				position_command(i, servo_position(i));
 				output_drive(i, 0);
 			}