File content as of revision 0:6b3497b2f2ec:

#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;
  }
}