sinyuu masahito
/
yozakura_arm
Arm control program for Yozakura
Diff: main.cpp
- Revision:
- 0:6b3497b2f2ec
diff -r 000000000000 -r 6b3497b2f2ec main.cpp --- /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; + } +}