Rauno U / Mbed 2 deprecated Miisu

Six crescent shaped legs

Dependencies:   mbed

main.cpp@47:4f418a4b0051, 2016-06-21 (annotated)

Committer:
sim642
Date:
Tue Jun 21 14:43:44 2016 +0000
Revision:
47:4f418a4b0051
Parent:
46:49f3da891e24 
Byte based communication

Who changed what in which revision?

UserRevisionLine numberNew contents of line
phairero 0:0b7259fdb68a 1 #include "mbed.h"
sim642 3:2235787e78c4 2 #include "EncoderMotor.hpp"
phairero 0:0b7259fdb68a 3
phairero 0:0b7259fdb68a 4 InterruptIn bt(USER_BUTTON);
phairero 30:476ab400d765 5 Serial pc(PA_0, PA_1); //RF
phairero 30:476ab400d765 6 //Serial pc(SERIAL_TX, SERIAL_RX);
phairero 26:c865244ca3cf 7 //Serial pc(USBTX, USBRX);
sim642 2:cf0147952fb9 8
sim642 23:d844cc906b66 9 //PIDData speedPIDData = {0.3f, 2.0f, 0.02f};
sim642 23:d844cc906b66 10 //PIDData turnPIDData = {5.0f, 0.1f, 0.04f};
sim642 23:d844cc906b66 11 PIDData speedPIDData = {0.5f, 0.0f, 0.0f};
phairero 26:c865244ca3cf 12 PIDData turnPIDData = {30.0f, 0.01f, 1.0f};
sim642 27:24a9ac72fe92 13
sim642 27:24a9ac72fe92 14 /*
sim642 27:24a9ac72fe92 15 PWM timer channel
sim642 27:24a9ac72fe92 16 M1 1 2N
sim642 27:24a9ac72fe92 17 M2 4 2
sim642 27:24a9ac72fe92 18 M3 2 1
sim642 27:24a9ac72fe92 19 M4 4 4
sim642 27:24a9ac72fe92 20 M5 1 3N
sim642 27:24a9ac72fe92 21 M6 2 2
sim642 27:24a9ac72fe92 22 */
phairero 26:c865244ca3cf 23
phairero 26:c865244ca3cf 24 // 1
sim642 39:e35a99801ec1 25 MotorData m1Data = {PB_0, PA_4, PC_0}; //PWM, Dir1, Dir2
sim642 39:e35a99801ec1 26 EncoderData enc1Data = {PC_3, PC_1, 102.083 * 64}; //EncA, encB // https://www.pololu.com/product/2826
sim642 41:8ce8a3a47a92 27 EncoderMotor m1(m1Data, enc1Data, speedPIDData, turnPIDData);
phairero 32:9db688917ca3 28 DigitalIn s1(PA_8);
phairero 26:c865244ca3cf 29 // 2
sim642 39:e35a99801ec1 30 MotorData m2Data = {PB_7, PA_6, PB_9}; //PB7 = fault dir2 oli enne PC13
sim642 39:e35a99801ec1 31 EncoderData enc2Data = {PC_14, PC_15, 102.083 * 64};
sim642 41:8ce8a3a47a92 32 EncoderMotor m2(m2Data, enc2Data, speedPIDData, turnPIDData);
phairero 32:9db688917ca3 33 DigitalIn s2(PH_1);
phairero 26:c865244ca3cf 34 // 3
sim642 35:bce9ac485178 35 MotorData m3Data = {PA_15, PC_12, PC_10};
sim642 35:bce9ac485178 36 EncoderData enc3Data = {PC_11, PA_9, 102.083 * 64}; //B oli varem PA_13
sim642 41:8ce8a3a47a92 37 EncoderMotor m3(m3Data, enc3Data, speedPIDData, turnPIDData);
phairero 32:9db688917ca3 38 DigitalIn s3(PA_14);
phairero 26:c865244ca3cf 39 // 4
phairero 26:c865244ca3cf 40 MotorData m4Data = {PB_8, PC_6, PC_9};
phairero 26:c865244ca3cf 41 EncoderData enc4Data = {PC_5, PA_12, 102.083 * 64};
sim642 41:8ce8a3a47a92 42 EncoderMotor m4(m4Data, enc4Data, speedPIDData, turnPIDData);
phairero 32:9db688917ca3 43 DigitalIn s4(PA_11);
phairero 26:c865244ca3cf 44 // 5
phairero 26:c865244ca3cf 45 MotorData m5Data = {PB_15, PB_1, PB_2};
phairero 33:ad2ebe22f8dd 46 EncoderData enc5Data = {PC_7, PB_6, 102.083 * 64}; //B oli varem PB_13, A oli varem PB_14
sim642 41:8ce8a3a47a92 47 EncoderMotor m5(m5Data, enc5Data, speedPIDData, turnPIDData);
phairero 32:9db688917ca3 48 DigitalIn s5(PC_4);
phairero 26:c865244ca3cf 49 // 6
sim642 39:e35a99801ec1 50 MotorData m6Data = {PB_3, PA_7, PB_5}; //PA_2 = TX; PA_3 (m6-fault) = RX DIR2 oli enne PA2
sim642 39:e35a99801ec1 51 EncoderData enc6Data = {PB_4, PA_10, 102.083 * 64};
sim642 41:8ce8a3a47a92 52 EncoderMotor m6(m6Data, enc6Data, speedPIDData, turnPIDData);
phairero 32:9db688917ca3 53 DigitalIn s6(PB_10);
sim642 35:bce9ac485178 54
phairero 30:476ab400d765 55 const int MOTORS = 6;
phairero 30:476ab400d765 56 EncoderMotor* ms[MOTORS] = {&m1, &m2, &m3, &m4, &m5, &m6};
phairero 32:9db688917ca3 57 DigitalIn ss[MOTORS] = {s1, s2, s3, s4, s5, s6};
sim642 35:bce9ac485178 58
sim642 47:4f418a4b0051 59 float volatile rxSpeed;
sim642 47:4f418a4b0051 60 int volatile rxDir;
sim642 47:4f418a4b0051 61 int volatile rxTurn;
sim642 47:4f418a4b0051 62
phairero 43:0627a2245b9d 63 float volatile speed;
phairero 43:0627a2245b9d 64 int volatile dir;
phairero 43:0627a2245b9d 65 int volatile turn;
phairero 43:0627a2245b9d 66
sim642 27:24a9ac72fe92 67 Ticker ticker;
sim642 27:24a9ac72fe92 68
sim642 2:cf0147952fb9 69 void rise()
sim642 2:cf0147952fb9 70 {
sim642 24:fb1827be6f7e 71 //pc.printf("rise\n");
sim642 28:fd21faad6dd8 72 //m1.drive(0);
phairero 36:906323851366 73 //mod = mod*(-1);
sim642 2:cf0147952fb9 74 }
phairero 0:0b7259fdb68a 75
sim642 2:cf0147952fb9 76 void fall()
sim642 2:cf0147952fb9 77 {
sim642 24:fb1827be6f7e 78 //pc.printf("fall\n");
sim642 28:fd21faad6dd8 79 //m1.drive(0.25);
sim642 2:cf0147952fb9 80 }
phairero 1:8b0322a353f4 81
sim642 27:24a9ac72fe92 82 void tick()
sim642 27:24a9ac72fe92 83 {
sim642 28:fd21faad6dd8 84 for (int i = 0; i < MOTORS; i++)
sim642 28:fd21faad6dd8 85 ms[i]->tick();
sim642 27:24a9ac72fe92 86 }
sim642 27:24a9ac72fe92 87
sim642 27:24a9ac72fe92 88 const float tickTime = 1.f / 60;
sim642 27:24a9ac72fe92 89
sim642 37:8021b3ce241a 90 void waitAllRotate()
sim642 37:8021b3ce241a 91 {
sim642 37:8021b3ce241a 92 tick();
sim642 37:8021b3ce241a 93
sim642 37:8021b3ce241a 94 float sum;
sim642 37:8021b3ce241a 95 do {
sim642 37:8021b3ce241a 96 sum = 0;
sim642 37:8021b3ce241a 97 for (int i = 0; i < MOTORS; i++)
sim642 37:8021b3ce241a 98 sum += abs(ms[i]->errorTurn);
phairero 45:409988ab53e4 99
sim642 47:4f418a4b0051 100 //pc.printf("");
sim642 47:4f418a4b0051 101 pc.printf("%f\n",sum);
sim642 37:8021b3ce241a 102 }
sim642 37:8021b3ce241a 103 while(sum > 0.05f);
sim642 37:8021b3ce241a 104
sim642 37:8021b3ce241a 105 wait(0.1f);
sim642 37:8021b3ce241a 106 }
sim642 37:8021b3ce241a 107
sim642 42:7fa713d5d1af 108 void calibrateLegs()
sim642 42:7fa713d5d1af 109 {
sim642 37:8021b3ce241a 110 for (int i = 0; i < MOTORS; i++)
sim642 37:8021b3ce241a 111 {
sim642 47:4f418a4b0051 112 ms[i]->drive(0.60f);
sim642 40:01a97bc4ef7a 113 }
sim642 40:01a97bc4ef7a 114
sim642 40:01a97bc4ef7a 115 bool done;
sim642 40:01a97bc4ef7a 116 do
sim642 40:01a97bc4ef7a 117 {
sim642 40:01a97bc4ef7a 118 done = true;
sim642 40:01a97bc4ef7a 119 for (int i = 0; i < MOTORS; i++)
sim642 40:01a97bc4ef7a 120 {
phairero 46:49f3da891e24 121 if (ss[i].read())
phairero 46:49f3da891e24 122 done = false;
phairero 46:49f3da891e24 123 else
phairero 46:49f3da891e24 124 ms[i]->drive(0.f);
sim642 37:8021b3ce241a 125 }
sim642 40:01a97bc4ef7a 126 }
sim642 40:01a97bc4ef7a 127 while (!done);
sim642 40:01a97bc4ef7a 128
sim642 40:01a97bc4ef7a 129 for (int i = 0; i < MOTORS; i++)
sim642 37:8021b3ce241a 130 ms[i]->getEncoder().reset();
sim642 42:7fa713d5d1af 131
phairero 43:0627a2245b9d 132 pc.printf("done\n");
sim642 42:7fa713d5d1af 133 }
sim642 42:7fa713d5d1af 134
sim642 42:7fa713d5d1af 135 void standUp()
sim642 42:7fa713d5d1af 136 {
phairero 44:c2acf8d5e191 137 ms[0]->rotate(0.5f + 0.0625f, 0.75f);
phairero 44:c2acf8d5e191 138 ms[1]->rotate(0.5f - 0.0625f, 0.75f);
phairero 44:c2acf8d5e191 139 ms[2]->rotate(0.5f + 0.0625f, 0.75f);
phairero 44:c2acf8d5e191 140 ms[3]->rotate(0.5f - 0.0625f, 0.75f);
phairero 44:c2acf8d5e191 141 ms[4]->rotate(0.5f + 0.0625f, 0.75f);
phairero 44:c2acf8d5e191 142 ms[5]->rotate(0.5f - 0.0625f, 0.75f);
sim642 37:8021b3ce241a 143 waitAllRotate();
sim642 42:7fa713d5d1af 144 }
sim642 42:7fa713d5d1af 145
sim642 47:4f418a4b0051 146 float groundDivide = 4;
sim642 47:4f418a4b0051 147
phairero 43:0627a2245b9d 148 void moveOne()
phairero 43:0627a2245b9d 149 {
phairero 44:c2acf8d5e191 150
phairero 44:c2acf8d5e191 151 ms[0]->rotate(dir*0.875f,speed);
sim642 47:4f418a4b0051 152 ms[1]->rotate(dir*0.125f,speed/groundDivide);
phairero 44:c2acf8d5e191 153 ms[2]->rotate(dir*0.875f,speed);
sim642 47:4f418a4b0051 154 ms[3]->rotate(dir*0.125f,speed/groundDivide);
phairero 44:c2acf8d5e191 155 ms[4]->rotate(dir*0.875f,speed);
sim642 47:4f418a4b0051 156 ms[5]->rotate(dir*0.125f,speed/groundDivide);
phairero 43:0627a2245b9d 157 waitAllRotate();
phairero 43:0627a2245b9d 158 }
phairero 43:0627a2245b9d 159
phairero 43:0627a2245b9d 160 void moveTwo(){
sim642 47:4f418a4b0051 161 ms[0]->rotate(dir*0.125f,speed/groundDivide);
phairero 44:c2acf8d5e191 162 ms[1]->rotate(dir*0.875f,speed);
sim642 47:4f418a4b0051 163 ms[2]->rotate(dir*0.125f,speed/groundDivide);
phairero 44:c2acf8d5e191 164 ms[3]->rotate(dir*0.875f,speed);
sim642 47:4f418a4b0051 165 ms[4]->rotate(dir*0.125f,speed/groundDivide);
phairero 44:c2acf8d5e191 166 ms[5]->rotate(dir*0.875f,speed);
phairero 44:c2acf8d5e191 167 waitAllRotate();
phairero 44:c2acf8d5e191 168 }
phairero 44:c2acf8d5e191 169
phairero 44:c2acf8d5e191 170 void turnOne(){
phairero 44:c2acf8d5e191 171 ms[0]->rotate(turn*dir*0.875f,speed);
sim642 47:4f418a4b0051 172 ms[1]->rotate(turn*dir*0.125f,speed/groundDivide);
sim642 47:4f418a4b0051 173 ms[2]->rotate(-turn*dir*0.125f,speed/groundDivide);
phairero 44:c2acf8d5e191 174 ms[3]->rotate(-turn*dir*0.875f,speed);
sim642 47:4f418a4b0051 175 ms[4]->rotate(-turn*dir*0.125f,speed/groundDivide);
sim642 47:4f418a4b0051 176 ms[5]->rotate(turn*dir*0.125f,speed/groundDivide);
phairero 44:c2acf8d5e191 177 waitAllRotate();
phairero 44:c2acf8d5e191 178 }
phairero 44:c2acf8d5e191 179
phairero 44:c2acf8d5e191 180 void turnTwo(){
sim642 47:4f418a4b0051 181 ms[0]->rotate(turn*dir*0.125f,speed/groundDivide);
phairero 45:409988ab53e4 182 ms[1]->rotate(0.0f, 1.0f);
phairero 44:c2acf8d5e191 183 ms[2]->rotate(-turn*dir*0.875f,speed);
phairero 45:409988ab53e4 184 ms[3]->rotate(0.0f, 1.0f);
phairero 44:c2acf8d5e191 185 ms[4]->rotate(-turn*dir*0.875f,speed);
phairero 45:409988ab53e4 186 ms[5]->rotate(0.0f, 1.0f);
phairero 44:c2acf8d5e191 187 waitAllRotate();
phairero 44:c2acf8d5e191 188 }
phairero 44:c2acf8d5e191 189
phairero 44:c2acf8d5e191 190 void turnThree(){
phairero 45:409988ab53e4 191 ms[0]->rotate(0.0f, 1.0f);
phairero 44:c2acf8d5e191 192 ms[1]->rotate(turn*dir*0.875f,speed);
phairero 45:409988ab53e4 193 ms[2]->rotate(0.0f, 1.0f);
sim642 47:4f418a4b0051 194 ms[3]->rotate(-turn*dir*0.125f,speed/groundDivide);
phairero 45:409988ab53e4 195 ms[4]->rotate(0.0f, 1.0f);
phairero 44:c2acf8d5e191 196 ms[5]->rotate(turn*dir*0.875f,speed);
phairero 43:0627a2245b9d 197 waitAllRotate();
phairero 43:0627a2245b9d 198 }
phairero 43:0627a2245b9d 199
sim642 47:4f418a4b0051 200 void rfRx()
sim642 47:4f418a4b0051 201 {
sim642 47:4f418a4b0051 202 int b = pc.getc();
sim642 47:4f418a4b0051 203 int rfSpeed = b & 0x3F;
sim642 47:4f418a4b0051 204
sim642 47:4f418a4b0051 205 rxSpeed = rfSpeed / 10.f * 2;
sim642 47:4f418a4b0051 206
sim642 47:4f418a4b0051 207 switch ((b >> 6) & 0x3)
sim642 47:4f418a4b0051 208 {
sim642 47:4f418a4b0051 209 case 0: // fwd
sim642 47:4f418a4b0051 210 rxDir = 1;
sim642 47:4f418a4b0051 211 rxTurn = 0;
sim642 47:4f418a4b0051 212 break;
sim642 47:4f418a4b0051 213
sim642 47:4f418a4b0051 214 case 1: // bwd
sim642 47:4f418a4b0051 215 rxDir = -1;
sim642 47:4f418a4b0051 216 rxTurn = 0;
sim642 47:4f418a4b0051 217 break;
sim642 47:4f418a4b0051 218
sim642 47:4f418a4b0051 219 case 2: // left
sim642 47:4f418a4b0051 220 rxDir = 1;
sim642 47:4f418a4b0051 221 rxTurn = -1;
sim642 47:4f418a4b0051 222 break;
sim642 47:4f418a4b0051 223
sim642 47:4f418a4b0051 224 case 3: // right
sim642 47:4f418a4b0051 225 rxDir = 1;
sim642 47:4f418a4b0051 226 rxTurn = 1;
sim642 47:4f418a4b0051 227 break;
sim642 47:4f418a4b0051 228 }
sim642 47:4f418a4b0051 229 }
phairero 43:0627a2245b9d 230
sim642 42:7fa713d5d1af 231 int main()
sim642 47:4f418a4b0051 232 {
sim642 47:4f418a4b0051 233 pc.attach(&rfRx);
sim642 47:4f418a4b0051 234
sim642 42:7fa713d5d1af 235 pc.printf("MAIN\n");
sim642 42:7fa713d5d1af 236 bt.rise(&rise);
sim642 42:7fa713d5d1af 237 bt.fall(&fall);
sim642 42:7fa713d5d1af 238
sim642 42:7fa713d5d1af 239 //ms[active]->drive(0.25);
sim642 42:7fa713d5d1af 240
sim642 42:7fa713d5d1af 241 ticker.attach(&tick, tickTime);
sim642 42:7fa713d5d1af 242
sim642 42:7fa713d5d1af 243 //unsigned char volatile rfget;
sim642 42:7fa713d5d1af 244
sim642 42:7fa713d5d1af 245 calibrateLegs();
sim642 42:7fa713d5d1af 246 standUp();
sim642 37:8021b3ce241a 247
phairero 43:0627a2245b9d 248 //float volatile sum;
phairero 36:906323851366 249
sim642 47:4f418a4b0051 250 rxSpeed = 0.0f;
sim642 47:4f418a4b0051 251 rxDir = 1;
sim642 47:4f418a4b0051 252 rxTurn = 0;
phairero 36:906323851366 253
phairero 43:0627a2245b9d 254 pc.printf("start\n");
sim642 2:cf0147952fb9 255 while(1)
phairero 36:906323851366 256 {
sim642 47:4f418a4b0051 257 speed = rxSpeed;
sim642 47:4f418a4b0051 258 dir = rxDir;
sim642 47:4f418a4b0051 259 turn = rxTurn;
sim642 47:4f418a4b0051 260
sim642 47:4f418a4b0051 261 //pc.printf("%.2f %d %d\n", speed, dir, turn);
phairero 43:0627a2245b9d 262
phairero 36:906323851366 263 if (speed != 0){
phairero 43:0627a2245b9d 264 //for (int i = 0; i < MOTORS; i++)
phairero 43:0627a2245b9d 265 // pc.printf("%ld ", ms[i]->encoder.getCount());
phairero 43:0627a2245b9d 266 //pc.printf("\n");
phairero 43:0627a2245b9d 267
phairero 43:0627a2245b9d 268 //speed = (rfget*0.3f); //+ ((speed < 0) ? -1 : 1) * mod;
phairero 44:c2acf8d5e191 269 if (turn == 0) {
phairero 44:c2acf8d5e191 270 if (dir == 1){
phairero 44:c2acf8d5e191 271 moveOne();
phairero 44:c2acf8d5e191 272 moveTwo();
phairero 44:c2acf8d5e191 273 }
phairero 44:c2acf8d5e191 274 else {
phairero 44:c2acf8d5e191 275 moveTwo();
phairero 44:c2acf8d5e191 276 moveOne();
phairero 43:0627a2245b9d 277 }
phairero 44:c2acf8d5e191 278 }
phairero 43:0627a2245b9d 279 else {
phairero 44:c2acf8d5e191 280 if ((turn*dir) == 1){
phairero 44:c2acf8d5e191 281 turnOne();
phairero 44:c2acf8d5e191 282 turnTwo();
phairero 44:c2acf8d5e191 283 turnThree();
phairero 43:0627a2245b9d 284 }
phairero 44:c2acf8d5e191 285 else {
phairero 44:c2acf8d5e191 286 turnThree();
phairero 44:c2acf8d5e191 287 turnTwo();
phairero 44:c2acf8d5e191 288 turnOne();
phairero 44:c2acf8d5e191 289 }
phairero 44:c2acf8d5e191 290 }
phairero 36:906323851366 291 }
phairero 36:906323851366 292 else {
phairero 45:409988ab53e4 293 ms[0]->rotate(0.0f, 1.0f);
phairero 45:409988ab53e4 294 ms[1]->rotate(0.0f, 1.0f);
phairero 45:409988ab53e4 295 ms[2]->rotate(0.0f, 1.0f);
phairero 45:409988ab53e4 296 ms[3]->rotate(0.0f, 1.0f);
phairero 45:409988ab53e4 297 ms[4]->rotate(0.0f, 1.0f);
phairero 45:409988ab53e4 298 ms[5]->rotate(0.0f, 1.0f);
phairero 36:906323851366 299 }
phairero 36:906323851366 300
phairero 36:906323851366 301 //pc.printf("%f", speed);
phairero 32:9db688917ca3 302 //info enkoodritelt
phairero 36:906323851366 303 /*
sim642 29:f905e66e1a6f 304 for (int i = 0; i < MOTORS; i++)
sim642 29:f905e66e1a6f 305 pc.printf("%ld ", ms[i]->encoder.getCount());
sim642 29:f905e66e1a6f 306 pc.printf("\n");
phairero 36:906323851366 307 */
sim642 37:8021b3ce241a 308
phairero 32:9db688917ca3 309 //info IRidelt
sim642 37:8021b3ce241a 310 /*for (int i = 0; i < MOTORS; i++)
phairero 32:9db688917ca3 311 pc.printf("%d ", ss[i].read());
phairero 32:9db688917ca3 312 pc.printf("\n");
sim642 37:8021b3ce241a 313 wait(1.0f);*/
sim642 37:8021b3ce241a 314
sim642 28:fd21faad6dd8 315 //pc.printf("%d", active);
phairero 31:8d8d31b6d8cd 316 //pc.scanf("%f", &turn);
sim642 23:d844cc906b66 317 //m.rotate(turn, 0.2);
phairero 31:8d8d31b6d8cd 318 //pc.scanf("%f %f", &rot, &speed);
sim642 28:fd21faad6dd8 319 //m1.rotate(rot, speed);
sim642 23:d844cc906b66 320 //m.drive(speed);
phairero 31:8d8d31b6d8cd 321 //pc.printf("%f %f\n", s.getTurnSpeed(), m.getSetSpeed());
phairero 31:8d8d31b6d8cd 322 //pc.printf("%f %f\n", m.s, m.getSetSpeed());
phairero 31:8d8d31b6d8cd 323 //pc.printf("%ld %f %f\n", m1.getEncoder().getCount(), m1.getEncoder().getTurn(), m1.getSetTurn());
phairero 31:8d8d31b6d8cd 324 //pc.printf("%f %f\n", m.getEncoder().getTurn(), m.getSetTurn());
phairero 36:906323851366 325
phairero 0:0b7259fdb68a 326 }
phairero 26:c865244ca3cf 327 }