Diff: main.cpp

Revision:

0:6b3497b2f2ec

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Apr 24 01:55:32 2015 +0000
@@ -0,0 +1,234 @@
+#include <vector>
+#include "mbed.h"
+#include "Dynamixel.h"
+#include "MEMS.h"
+
+struct ArmPacketBits {
+                      unsigned int mode : 2;
+                      unsigned int linear : 2;
+                      unsigned int pitch : 2;
+                      unsigned int yaw : 2;
+                     };
+
+union ArmPacket {
+                 struct ArmPacketBits b;
+                 unsigned char as_byte;
+                };
+
+Serial rpi(USBTX, USBRX);
+
+AnalogIn co2(p20);
+
+DigitalOut dx_low(p16);
+DigitalOut dx_relay(p18);
+
+DigitalOut led_1(LED1);
+DigitalOut led_2(LED2);
+DigitalOut led_3(LED3);
+DigitalOut led_4(LED4);
+
+std::vector<Dynamixel*> servos;
+AX12 *linear = new AX12(p13, p14, 0);
+MX28 *pitch = new MX28(p13, p14, 1);
+MX28 *yaw = new MX28(p13, p14, 2);
+
+MEMS thermo_sensors[] = { MEMS(p9, p10),
+                          MEMS(p28, p27) };
+
+int minima[] = {100, 172, 360};
+int maxima[] = {300, 334, 360};
+float speeds[] = {0.1, 0.2, 0.2};
+int inits[] = {maxima[0], maxima[1], 0};
+int goals[] = {maxima[0], maxima[1], 0};
+
+void DxGoHome() {
+  led_2 = 1;
+  if (abs(linear->GetPosition() - inits[0]) < 2) {
+    pitch->SetGoal(inits[1]);
+    yaw->SetGoal(inits[2]);
+  } else {
+    linear->SetGoal(inits[0]);
+  }
+  led_2 = 0;
+}
+
+void DxGoHomeLoop() {
+  led_4 = 1;
+  led_2 = 1;
+  led_1 = 1;
+  linear->SetGoal(inits[0]);
+  wait(2);
+  led_1 = 0;
+  pitch->SetGoal(inits[1]);
+  yaw->SetGoal(inits[2]);
+  led_2 = 1;
+  led_3 = 1;
+  wait(5);
+  led_2 = 0;
+  led_3 = 0;
+  led_4 = 0;
+}
+
+void DxInitialize() {
+  led_1 = 1;
+  dx_low = 0;
+  dx_relay = 1;
+  
+  for (int i=0; i < 3; i++) {
+    servos[i]->SetCWLimit(minima[i]);
+    servos[i]->SetCCWLimit(maxima[i]);
+    servos[i]->SetCRSpeed(speeds[i]);
+  }
+  led_1 = 0;
+}
+
+void DxReset() {
+  led_4 = 0;
+  led_3 = 1;
+  dx_relay = 0;
+  wait_ms(10);
+  DxInitialize();
+  led_3 = 0;
+}
+
+void DxEnd() {
+  DxGoHomeLoop();
+  dx_relay = 0;
+  led_4 = 1;
+}
+
+float GetCO2() {
+  return co2.read() * 5000 + 400;  // ppm
+}
+
+int main() {
+  led_1 = 1;
+  led_2 = 1;
+  led_3 = 1;
+  led_4 = 1;
+  
+  servos.push_back(linear);
+  servos.push_back(pitch);
+  servos.push_back(yaw);
+  
+  float positions[3];
+  float values[3];
+  float thermo_data[2][16];
+  float co2_data;
+  int commands[3];
+
+  union ArmPacket packet;
+
+  rpi.baud(38400);  // Match this in the RPi settings.
+  led_1 = 0;
+  led_2 = 0;
+  led_3 = 0;
+  led_4 = 0;
+  
+  DxInitialize();  // Comment this out when testing without the arm.
+  DxGoHomeLoop();
+  
+  while (1) {
+    led_3 = 1;
+    packet.as_byte = rpi.getc();
+    led_3 = 0;
+
+    for (int i = 0; i < 3; i++) {
+      positions[i] = -1;
+      values[i] = -1;
+    }
+
+    commands[0] = packet.b.linear;
+    commands[1] = packet.b.pitch;
+    commands[2] = packet.b.yaw;
+
+    rpi.printf("%d %d %d %d\n", packet.b.mode, packet.b.linear, packet.b.pitch, packet.b.yaw);
+    
+    switch (packet.b.mode) {
+      case 0: {
+        if (not dx_relay) {
+          break;
+        }
+        
+        linear->SetTorqueLimit(1);  // Reset torque limit
+        led_3 = 1;
+        led_4 = 1;
+        for (int i=0; i < 3; i++) {
+          positions[i] = servos[i]->GetPosition();
+          if (i == 0) {
+            values[i] = servos[i]->GetVolts();
+          } else {
+            values[i] = servos[i]->GetCurrent();
+          }
+          
+          if (commands[i] == 1) {
+            servos[i]->SetGoal(positions[i] + 1);
+          } else if (commands[i] == 2) {
+            servos[i]->SetGoal(positions[i] - 1);
+          }
+        }
+        led_3 = 0;
+        led_4 = 0;
+        break;
+      } case 1: {
+        if (dx_relay) {
+          DxGoHomeLoop();
+        }
+        break;
+      } case 2: {
+        DxReset();
+        break;
+      } case 3: {
+        if (packet.b.linear) {
+          led_1 = 1;
+          led_2 = 1;
+          rpi.printf("arm\n");
+          led_1 = 0;
+          led_2 = 0;
+        } else {
+          if (dx_relay) {
+            DxEnd();
+          }
+        }
+        break;
+      }
+    }
+    
+    led_1 = 1;
+    led_2 = 1;
+    for (int i=0; i < 2; i++) {
+      thermo_sensors[i].temp(thermo_data[i]);
+    }
+    
+    co2_data = GetCO2();
+    led_1 = 0;
+    led_2 = 0;
+    
+    led_2 = 1;
+    led_3 = 1;
+    // Send Dynamixel position
+    for (int i=0; i < 3; i++) {
+      rpi.printf("%4.1f ", positions[i]);
+    }
+    
+    // Send Dynamixel values
+    for (int i=0; i < 3; i++) {
+      rpi.printf("%4.1f ", values[i]);
+    }
+    
+    // Send thermo data
+    for (int i=0; i < 2; i++) {
+      for (int j=0; j < 16; j++) {
+        rpi.printf("%4.1f ", thermo_data[i][j]);
+      }
+    }
+    
+    // Send CO2 data
+    rpi.printf("%4.1f", co2_data);
+    
+    // End transmission
+    rpi.printf("\n");
+    led_2 = 0;
+    led_3 = 0;
+  }
+}