sinyuu masahito / Mbed 2 deprecated yozakura_arm

Arm control program for Yozakura

Dependencies:   mbed

main.cpp@0:6b3497b2f2ec, 2015-04-24 (annotated)

Committer:
masasin
Date:
Fri Apr 24 01:55:32 2015 +0000
Revision:
0:6b3497b2f2ec
Initial commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
masasin 0:6b3497b2f2ec 1 #include <vector>
masasin 0:6b3497b2f2ec 2 #include "mbed.h"
masasin 0:6b3497b2f2ec 3 #include "Dynamixel.h"
masasin 0:6b3497b2f2ec 4 #include "MEMS.h"
masasin 0:6b3497b2f2ec 5
masasin 0:6b3497b2f2ec 6 struct ArmPacketBits {
masasin 0:6b3497b2f2ec 7 unsigned int mode : 2;
masasin 0:6b3497b2f2ec 8 unsigned int linear : 2;
masasin 0:6b3497b2f2ec 9 unsigned int pitch : 2;
masasin 0:6b3497b2f2ec 10 unsigned int yaw : 2;
masasin 0:6b3497b2f2ec 11 };
masasin 0:6b3497b2f2ec 12
masasin 0:6b3497b2f2ec 13 union ArmPacket {
masasin 0:6b3497b2f2ec 14 struct ArmPacketBits b;
masasin 0:6b3497b2f2ec 15 unsigned char as_byte;
masasin 0:6b3497b2f2ec 16 };
masasin 0:6b3497b2f2ec 17
masasin 0:6b3497b2f2ec 18 Serial rpi(USBTX, USBRX);
masasin 0:6b3497b2f2ec 19
masasin 0:6b3497b2f2ec 20 AnalogIn co2(p20);
masasin 0:6b3497b2f2ec 21
masasin 0:6b3497b2f2ec 22 DigitalOut dx_low(p16);
masasin 0:6b3497b2f2ec 23 DigitalOut dx_relay(p18);
masasin 0:6b3497b2f2ec 24
masasin 0:6b3497b2f2ec 25 DigitalOut led_1(LED1);
masasin 0:6b3497b2f2ec 26 DigitalOut led_2(LED2);
masasin 0:6b3497b2f2ec 27 DigitalOut led_3(LED3);
masasin 0:6b3497b2f2ec 28 DigitalOut led_4(LED4);
masasin 0:6b3497b2f2ec 29
masasin 0:6b3497b2f2ec 30 std::vector<Dynamixel*> servos;
masasin 0:6b3497b2f2ec 31 AX12 *linear = new AX12(p13, p14, 0);
masasin 0:6b3497b2f2ec 32 MX28 *pitch = new MX28(p13, p14, 1);
masasin 0:6b3497b2f2ec 33 MX28 *yaw = new MX28(p13, p14, 2);
masasin 0:6b3497b2f2ec 34
masasin 0:6b3497b2f2ec 35 MEMS thermo_sensors[] = { MEMS(p9, p10),
masasin 0:6b3497b2f2ec 36 MEMS(p28, p27) };
masasin 0:6b3497b2f2ec 37
masasin 0:6b3497b2f2ec 38 int minima[] = {100, 172, 360};
masasin 0:6b3497b2f2ec 39 int maxima[] = {300, 334, 360};
masasin 0:6b3497b2f2ec 40 float speeds[] = {0.1, 0.2, 0.2};
masasin 0:6b3497b2f2ec 41 int inits[] = {maxima[0], maxima[1], 0};
masasin 0:6b3497b2f2ec 42 int goals[] = {maxima[0], maxima[1], 0};
masasin 0:6b3497b2f2ec 43
masasin 0:6b3497b2f2ec 44 void DxGoHome() {
masasin 0:6b3497b2f2ec 45 led_2 = 1;
masasin 0:6b3497b2f2ec 46 if (abs(linear->GetPosition() - inits[0]) < 2) {
masasin 0:6b3497b2f2ec 47 pitch->SetGoal(inits[1]);
masasin 0:6b3497b2f2ec 48 yaw->SetGoal(inits[2]);
masasin 0:6b3497b2f2ec 49 } else {
masasin 0:6b3497b2f2ec 50 linear->SetGoal(inits[0]);
masasin 0:6b3497b2f2ec 51 }
masasin 0:6b3497b2f2ec 52 led_2 = 0;
masasin 0:6b3497b2f2ec 53 }
masasin 0:6b3497b2f2ec 54
masasin 0:6b3497b2f2ec 55 void DxGoHomeLoop() {
masasin 0:6b3497b2f2ec 56 led_4 = 1;
masasin 0:6b3497b2f2ec 57 led_2 = 1;
masasin 0:6b3497b2f2ec 58 led_1 = 1;
masasin 0:6b3497b2f2ec 59 linear->SetGoal(inits[0]);
masasin 0:6b3497b2f2ec 60 wait(2);
masasin 0:6b3497b2f2ec 61 led_1 = 0;
masasin 0:6b3497b2f2ec 62 pitch->SetGoal(inits[1]);
masasin 0:6b3497b2f2ec 63 yaw->SetGoal(inits[2]);
masasin 0:6b3497b2f2ec 64 led_2 = 1;
masasin 0:6b3497b2f2ec 65 led_3 = 1;
masasin 0:6b3497b2f2ec 66 wait(5);
masasin 0:6b3497b2f2ec 67 led_2 = 0;
masasin 0:6b3497b2f2ec 68 led_3 = 0;
masasin 0:6b3497b2f2ec 69 led_4 = 0;
masasin 0:6b3497b2f2ec 70 }
masasin 0:6b3497b2f2ec 71
masasin 0:6b3497b2f2ec 72 void DxInitialize() {
masasin 0:6b3497b2f2ec 73 led_1 = 1;
masasin 0:6b3497b2f2ec 74 dx_low = 0;
masasin 0:6b3497b2f2ec 75 dx_relay = 1;
masasin 0:6b3497b2f2ec 76
masasin 0:6b3497b2f2ec 77 for (int i=0; i < 3; i++) {
masasin 0:6b3497b2f2ec 78 servos[i]->SetCWLimit(minima[i]);
masasin 0:6b3497b2f2ec 79 servos[i]->SetCCWLimit(maxima[i]);
masasin 0:6b3497b2f2ec 80 servos[i]->SetCRSpeed(speeds[i]);
masasin 0:6b3497b2f2ec 81 }
masasin 0:6b3497b2f2ec 82 led_1 = 0;
masasin 0:6b3497b2f2ec 83 }
masasin 0:6b3497b2f2ec 84
masasin 0:6b3497b2f2ec 85 void DxReset() {
masasin 0:6b3497b2f2ec 86 led_4 = 0;
masasin 0:6b3497b2f2ec 87 led_3 = 1;
masasin 0:6b3497b2f2ec 88 dx_relay = 0;
masasin 0:6b3497b2f2ec 89 wait_ms(10);
masasin 0:6b3497b2f2ec 90 DxInitialize();
masasin 0:6b3497b2f2ec 91 led_3 = 0;
masasin 0:6b3497b2f2ec 92 }
masasin 0:6b3497b2f2ec 93
masasin 0:6b3497b2f2ec 94 void DxEnd() {
masasin 0:6b3497b2f2ec 95 DxGoHomeLoop();
masasin 0:6b3497b2f2ec 96 dx_relay = 0;
masasin 0:6b3497b2f2ec 97 led_4 = 1;
masasin 0:6b3497b2f2ec 98 }
masasin 0:6b3497b2f2ec 99
masasin 0:6b3497b2f2ec 100 float GetCO2() {
masasin 0:6b3497b2f2ec 101 return co2.read() * 5000 + 400; // ppm
masasin 0:6b3497b2f2ec 102 }
masasin 0:6b3497b2f2ec 103
masasin 0:6b3497b2f2ec 104 int main() {
masasin 0:6b3497b2f2ec 105 led_1 = 1;
masasin 0:6b3497b2f2ec 106 led_2 = 1;
masasin 0:6b3497b2f2ec 107 led_3 = 1;
masasin 0:6b3497b2f2ec 108 led_4 = 1;
masasin 0:6b3497b2f2ec 109
masasin 0:6b3497b2f2ec 110 servos.push_back(linear);
masasin 0:6b3497b2f2ec 111 servos.push_back(pitch);
masasin 0:6b3497b2f2ec 112 servos.push_back(yaw);
masasin 0:6b3497b2f2ec 113
masasin 0:6b3497b2f2ec 114 float positions[3];
masasin 0:6b3497b2f2ec 115 float values[3];
masasin 0:6b3497b2f2ec 116 float thermo_data[2][16];
masasin 0:6b3497b2f2ec 117 float co2_data;
masasin 0:6b3497b2f2ec 118 int commands[3];
masasin 0:6b3497b2f2ec 119
masasin 0:6b3497b2f2ec 120 union ArmPacket packet;
masasin 0:6b3497b2f2ec 121
masasin 0:6b3497b2f2ec 122 rpi.baud(38400); // Match this in the RPi settings.
masasin 0:6b3497b2f2ec 123 led_1 = 0;
masasin 0:6b3497b2f2ec 124 led_2 = 0;
masasin 0:6b3497b2f2ec 125 led_3 = 0;
masasin 0:6b3497b2f2ec 126 led_4 = 0;
masasin 0:6b3497b2f2ec 127
masasin 0:6b3497b2f2ec 128 DxInitialize(); // Comment this out when testing without the arm.
masasin 0:6b3497b2f2ec 129 DxGoHomeLoop();
masasin 0:6b3497b2f2ec 130
masasin 0:6b3497b2f2ec 131 while (1) {
masasin 0:6b3497b2f2ec 132 led_3 = 1;
masasin 0:6b3497b2f2ec 133 packet.as_byte = rpi.getc();
masasin 0:6b3497b2f2ec 134 led_3 = 0;
masasin 0:6b3497b2f2ec 135
masasin 0:6b3497b2f2ec 136 for (int i = 0; i < 3; i++) {
masasin 0:6b3497b2f2ec 137 positions[i] = -1;
masasin 0:6b3497b2f2ec 138 values[i] = -1;
masasin 0:6b3497b2f2ec 139 }
masasin 0:6b3497b2f2ec 140
masasin 0:6b3497b2f2ec 141 commands[0] = packet.b.linear;
masasin 0:6b3497b2f2ec 142 commands[1] = packet.b.pitch;
masasin 0:6b3497b2f2ec 143 commands[2] = packet.b.yaw;
masasin 0:6b3497b2f2ec 144
masasin 0:6b3497b2f2ec 145 rpi.printf("%d %d %d %d\n", packet.b.mode, packet.b.linear, packet.b.pitch, packet.b.yaw);
masasin 0:6b3497b2f2ec 146
masasin 0:6b3497b2f2ec 147 switch (packet.b.mode) {
masasin 0:6b3497b2f2ec 148 case 0: {
masasin 0:6b3497b2f2ec 149 if (not dx_relay) {
masasin 0:6b3497b2f2ec 150 break;
masasin 0:6b3497b2f2ec 151 }
masasin 0:6b3497b2f2ec 152
masasin 0:6b3497b2f2ec 153 linear->SetTorqueLimit(1); // Reset torque limit
masasin 0:6b3497b2f2ec 154 led_3 = 1;
masasin 0:6b3497b2f2ec 155 led_4 = 1;
masasin 0:6b3497b2f2ec 156 for (int i=0; i < 3; i++) {
masasin 0:6b3497b2f2ec 157 positions[i] = servos[i]->GetPosition();
masasin 0:6b3497b2f2ec 158 if (i == 0) {
masasin 0:6b3497b2f2ec 159 values[i] = servos[i]->GetVolts();
masasin 0:6b3497b2f2ec 160 } else {
masasin 0:6b3497b2f2ec 161 values[i] = servos[i]->GetCurrent();
masasin 0:6b3497b2f2ec 162 }
masasin 0:6b3497b2f2ec 163
masasin 0:6b3497b2f2ec 164 if (commands[i] == 1) {
masasin 0:6b3497b2f2ec 165 servos[i]->SetGoal(positions[i] + 1);
masasin 0:6b3497b2f2ec 166 } else if (commands[i] == 2) {
masasin 0:6b3497b2f2ec 167 servos[i]->SetGoal(positions[i] - 1);
masasin 0:6b3497b2f2ec 168 }
masasin 0:6b3497b2f2ec 169 }
masasin 0:6b3497b2f2ec 170 led_3 = 0;
masasin 0:6b3497b2f2ec 171 led_4 = 0;
masasin 0:6b3497b2f2ec 172 break;
masasin 0:6b3497b2f2ec 173 } case 1: {
masasin 0:6b3497b2f2ec 174 if (dx_relay) {
masasin 0:6b3497b2f2ec 175 DxGoHomeLoop();
masasin 0:6b3497b2f2ec 176 }
masasin 0:6b3497b2f2ec 177 break;
masasin 0:6b3497b2f2ec 178 } case 2: {
masasin 0:6b3497b2f2ec 179 DxReset();
masasin 0:6b3497b2f2ec 180 break;
masasin 0:6b3497b2f2ec 181 } case 3: {
masasin 0:6b3497b2f2ec 182 if (packet.b.linear) {
masasin 0:6b3497b2f2ec 183 led_1 = 1;
masasin 0:6b3497b2f2ec 184 led_2 = 1;
masasin 0:6b3497b2f2ec 185 rpi.printf("arm\n");
masasin 0:6b3497b2f2ec 186 led_1 = 0;
masasin 0:6b3497b2f2ec 187 led_2 = 0;
masasin 0:6b3497b2f2ec 188 } else {
masasin 0:6b3497b2f2ec 189 if (dx_relay) {
masasin 0:6b3497b2f2ec 190 DxEnd();
masasin 0:6b3497b2f2ec 191 }
masasin 0:6b3497b2f2ec 192 }
masasin 0:6b3497b2f2ec 193 break;
masasin 0:6b3497b2f2ec 194 }
masasin 0:6b3497b2f2ec 195 }
masasin 0:6b3497b2f2ec 196
masasin 0:6b3497b2f2ec 197 led_1 = 1;
masasin 0:6b3497b2f2ec 198 led_2 = 1;
masasin 0:6b3497b2f2ec 199 for (int i=0; i < 2; i++) {
masasin 0:6b3497b2f2ec 200 thermo_sensors[i].temp(thermo_data[i]);
masasin 0:6b3497b2f2ec 201 }
masasin 0:6b3497b2f2ec 202
masasin 0:6b3497b2f2ec 203 co2_data = GetCO2();
masasin 0:6b3497b2f2ec 204 led_1 = 0;
masasin 0:6b3497b2f2ec 205 led_2 = 0;
masasin 0:6b3497b2f2ec 206
masasin 0:6b3497b2f2ec 207 led_2 = 1;
masasin 0:6b3497b2f2ec 208 led_3 = 1;
masasin 0:6b3497b2f2ec 209 // Send Dynamixel position
masasin 0:6b3497b2f2ec 210 for (int i=0; i < 3; i++) {
masasin 0:6b3497b2f2ec 211 rpi.printf("%4.1f ", positions[i]);
masasin 0:6b3497b2f2ec 212 }
masasin 0:6b3497b2f2ec 213
masasin 0:6b3497b2f2ec 214 // Send Dynamixel values
masasin 0:6b3497b2f2ec 215 for (int i=0; i < 3; i++) {
masasin 0:6b3497b2f2ec 216 rpi.printf("%4.1f ", values[i]);
masasin 0:6b3497b2f2ec 217 }
masasin 0:6b3497b2f2ec 218
masasin 0:6b3497b2f2ec 219 // Send thermo data
masasin 0:6b3497b2f2ec 220 for (int i=0; i < 2; i++) {
masasin 0:6b3497b2f2ec 221 for (int j=0; j < 16; j++) {
masasin 0:6b3497b2f2ec 222 rpi.printf("%4.1f ", thermo_data[i][j]);
masasin 0:6b3497b2f2ec 223 }
masasin 0:6b3497b2f2ec 224 }
masasin 0:6b3497b2f2ec 225
masasin 0:6b3497b2f2ec 226 // Send CO2 data
masasin 0:6b3497b2f2ec 227 rpi.printf("%4.1f", co2_data);
masasin 0:6b3497b2f2ec 228
masasin 0:6b3497b2f2ec 229 // End transmission
masasin 0:6b3497b2f2ec 230 rpi.printf("\n");
masasin 0:6b3497b2f2ec 231 led_2 = 0;
masasin 0:6b3497b2f2ec 232 led_3 = 0;
masasin 0:6b3497b2f2ec 233 }
masasin 0:6b3497b2f2ec 234 }