Commits

Trammell Hudson  committed 02c9051 Draft

Multi output works

  • Participants
  • Parent commits 39495fe

Comments (0)

Files changed (1)

 
 
 static void
-position_loop(void)
+servo_loop(
+	const uint8_t id
+)
 {
-	const uint8_t i = 1;
-	servo_t * const servo = &servos[i];
+	servo_t * const servo = &servos[id];
 
 	const int p_gain =  8;
 	const int d_gain = 64;
 
-	int16_t position = servo_position(i);
+	int16_t position = servo_position(id);
 
 	int16_t error = servo->command - position;
 	if (error < -1000)
 		output = 127;
 		
 	if (output_enabled)
-		output_drive(i, output);
+		output_drive(id, output);
 }
 
 
 static void
-position_output(void)
+position_loop(void)
+{
+	for (int i = 0 ; i < 6 ; i++)
+		servo_loop(i);
+}
+
+
+static void
+position_output(
+	const uint8_t sel_id
+)
 {
 	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++] = ' ';
+	buf[off++] = hexdigit(sel_id);
 
-#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
+	for (int i = 0 ; i < 3 ; i++)
+	{
+		buf[off++] = ' ';
+		off += print_position(buf+off, servo_position(i));
+		off += print_position(buf+off, servos[i].velocity);
+	}
 
 	buf[off++] = '\r';
 	buf[off++] = '\n';
 	uint16_t val = 0;
 	uint8_t sign = 1;
 	uint8_t phase = 0;
+	uint8_t servo_id = 1;
 
 	while (1)
 	{
 			sbi(TIFR0, OCF0A); // reset the bit
 			position_loop();
 			if ((phase++ & 0xF) == 0)
-				position_output();
+				position_output(servo_id);
 		}
 
 		int c = usb_serial_getchar();
 		if (c == '\n')
 			continue;
 
+		if (c == '=')
+		{
+			servo_id = val & 7;
+			val = 0;
+			sign = 1;
+			continue;
+		}
+
 		if (c == '\r')
 		{
 			send_str(PSTR("+\r\n"));
-			position_command(1, sign ? val : -val);
+			position_command(servo_id, sign ? val : -val);
 			val = 0;
 			sign = 1;
 			continue;