Rauno U / Mbed 2 deprecated Miisu

Six crescent shaped legs

Dependencies:   mbed

main.cpp@41:8ce8a3a47a92, 2016-06-15 (annotated)

Committer:
sim642
Date:
Wed Jun 15 16:00:48 2016 +0000
Revision:
41:8ce8a3a47a92
Parent:
40:01a97bc4ef7a
Child:
42:7fa713d5d1af
Remove unused SyncGroup usage

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 36:906323851366 57 volatile float mod = 0.25;
phairero 32:9db688917ca3 58 DigitalIn ss[MOTORS] = {s1, s2, s3, s4, s5, s6};
sim642 35:bce9ac485178 59
sim642 27:24a9ac72fe92 60 Ticker ticker;
sim642 27:24a9ac72fe92 61
sim642 2:cf0147952fb9 62 void rise()
sim642 2:cf0147952fb9 63 {
sim642 24:fb1827be6f7e 64 //pc.printf("rise\n");
sim642 28:fd21faad6dd8 65 //m1.drive(0);
phairero 36:906323851366 66 //mod = mod*(-1);
sim642 2:cf0147952fb9 67 }
phairero 0:0b7259fdb68a 68
sim642 2:cf0147952fb9 69 void fall()
sim642 2:cf0147952fb9 70 {
sim642 24:fb1827be6f7e 71 //pc.printf("fall\n");
sim642 28:fd21faad6dd8 72 //m1.drive(0.25);
sim642 2:cf0147952fb9 73 }
phairero 1:8b0322a353f4 74
sim642 27:24a9ac72fe92 75 void tick()
sim642 27:24a9ac72fe92 76 {
sim642 28:fd21faad6dd8 77 for (int i = 0; i < MOTORS; i++)
sim642 28:fd21faad6dd8 78 ms[i]->tick();
sim642 27:24a9ac72fe92 79 }
sim642 27:24a9ac72fe92 80
sim642 27:24a9ac72fe92 81 const float tickTime = 1.f / 60;
sim642 27:24a9ac72fe92 82
sim642 37:8021b3ce241a 83 void waitAllRotate()
sim642 37:8021b3ce241a 84 {
sim642 37:8021b3ce241a 85 tick();
sim642 37:8021b3ce241a 86
sim642 37:8021b3ce241a 87 float sum;
sim642 37:8021b3ce241a 88 do {
sim642 37:8021b3ce241a 89 sum = 0;
sim642 37:8021b3ce241a 90 for (int i = 0; i < MOTORS; i++)
sim642 37:8021b3ce241a 91 sum += abs(ms[i]->errorTurn);
sim642 37:8021b3ce241a 92 pc.printf("%f\n",sum);
sim642 37:8021b3ce241a 93 }
sim642 37:8021b3ce241a 94 while(sum > 0.05f);
sim642 37:8021b3ce241a 95
sim642 37:8021b3ce241a 96 wait(0.1f);
sim642 37:8021b3ce241a 97 }
sim642 37:8021b3ce241a 98
sim642 2:cf0147952fb9 99 int main()
sim642 3:2235787e78c4 100 {
phairero 31:8d8d31b6d8cd 101 pc.printf("MAIN\n");
sim642 3:2235787e78c4 102 bt.rise(&rise);
sim642 3:2235787e78c4 103 bt.fall(&fall);
sim642 27:24a9ac72fe92 104
sim642 28:fd21faad6dd8 105 //ms[active]->drive(0.25);
sim642 28:fd21faad6dd8 106
sim642 27:24a9ac72fe92 107 ticker.attach(&tick, tickTime);
sim642 5:7f800f61cb13 108
phairero 36:906323851366 109 //unsigned char volatile rfget;
phairero 33:ad2ebe22f8dd 110
sim642 37:8021b3ce241a 111 for (int i = 0; i < MOTORS; i++)
sim642 37:8021b3ce241a 112 {
sim642 40:01a97bc4ef7a 113 if (i != 3)
sim642 39:e35a99801ec1 114 ms[i]->drive(0.2f);
sim642 40:01a97bc4ef7a 115 }
sim642 40:01a97bc4ef7a 116
sim642 40:01a97bc4ef7a 117 bool done;
sim642 40:01a97bc4ef7a 118 do
sim642 40:01a97bc4ef7a 119 {
sim642 40:01a97bc4ef7a 120 done = true;
sim642 40:01a97bc4ef7a 121 for (int i = 0; i < MOTORS; i++)
sim642 40:01a97bc4ef7a 122 {
sim642 40:01a97bc4ef7a 123 if (i != 3)
sim642 40:01a97bc4ef7a 124 {
sim642 40:01a97bc4ef7a 125 if (ss[i].read())
sim642 40:01a97bc4ef7a 126 done = false;
sim642 40:01a97bc4ef7a 127 else
sim642 40:01a97bc4ef7a 128 ms[i]->drive(0.f);
sim642 40:01a97bc4ef7a 129 }
sim642 37:8021b3ce241a 130 }
sim642 40:01a97bc4ef7a 131 }
sim642 40:01a97bc4ef7a 132 while (!done);
sim642 40:01a97bc4ef7a 133
sim642 40:01a97bc4ef7a 134 for (int i = 0; i < MOTORS; i++)
sim642 37:8021b3ce241a 135 ms[i]->getEncoder().reset();
sim642 37:8021b3ce241a 136 pc.printf("done");
sim642 37:8021b3ce241a 137
sim642 37:8021b3ce241a 138 for (int i = 0; i < MOTORS; i++)
sim642 37:8021b3ce241a 139 pc.printf("%ld ", ms[i]->encoder.getCount());
sim642 37:8021b3ce241a 140 pc.printf("\n");
sim642 37:8021b3ce241a 141
sim642 39:e35a99801ec1 142 ms[0]->rotate(0.5f + 0.125f, 0.75f);
sim642 39:e35a99801ec1 143 ms[1]->rotate(0.5f - 0.125f, 0.75f);
sim642 38:b03a5bf9ac7b 144 ms[2]->rotate(0.5f + 0.125f, 0.75f);
sim642 38:b03a5bf9ac7b 145 ms[3]->rotate(0.5f - 0.125f, 0.75f);
sim642 38:b03a5bf9ac7b 146 ms[4]->rotate(0.5f + 0.125f, 0.75f);
sim642 39:e35a99801ec1 147 ms[5]->rotate(0.5f - 0.125f, 0.75f);
sim642 37:8021b3ce241a 148 waitAllRotate();
sim642 37:8021b3ce241a 149
phairero 36:906323851366 150 float volatile speed;
phairero 36:906323851366 151
phairero 36:906323851366 152 float volatile sum;
phairero 36:906323851366 153
sim642 25:a8bb69e99d6b 154
sim642 2:cf0147952fb9 155 while(1)
phairero 36:906323851366 156 {
phairero 36:906323851366 157
sim642 38:b03a5bf9ac7b 158 speed = 1.5f;
sim642 37:8021b3ce241a 159 //speed = 0;
phairero 36:906323851366 160 //int oldactive = active;
phairero 36:906323851366 161 //pc.scanf("%f", &speed);
phairero 36:906323851366 162
phairero 36:906323851366 163 if (speed != 0){
sim642 37:8021b3ce241a 164 for (int i = 0; i < MOTORS; i++)
sim642 37:8021b3ce241a 165 pc.printf("%ld ", ms[i]->encoder.getCount());
sim642 37:8021b3ce241a 166 pc.printf("\n");
sim642 37:8021b3ce241a 167
phairero 36:906323851366 168 speed = (speed*0.3f); //+ ((speed < 0) ? -1 : 1) * mod;
phairero 36:906323851366 169
sim642 39:e35a99801ec1 170 ms[0]->rotate(0.75f,speed*3);
sim642 39:e35a99801ec1 171 ms[1]->rotate(0.25f,speed);
phairero 36:906323851366 172 ms[2]->rotate(0.75f,speed*3);
phairero 36:906323851366 173 ms[3]->rotate(0.25f,speed);
phairero 36:906323851366 174 ms[4]->rotate(0.75f,speed*3);
sim642 39:e35a99801ec1 175 ms[5]->rotate(0.25f,speed);
sim642 37:8021b3ce241a 176 waitAllRotate();
sim642 37:8021b3ce241a 177
sim642 39:e35a99801ec1 178 ms[0]->rotate(0.25f,speed);
sim642 39:e35a99801ec1 179 ms[1]->rotate(0.75f,speed*3);
phairero 36:906323851366 180 ms[2]->rotate(0.25f,speed);
phairero 36:906323851366 181 ms[3]->rotate(0.75f,speed*3);
phairero 36:906323851366 182 ms[4]->rotate(0.25f,speed);
sim642 39:e35a99801ec1 183 ms[5]->rotate(0.75f,speed*3);
sim642 37:8021b3ce241a 184 waitAllRotate();
phairero 36:906323851366 185 }
phairero 36:906323851366 186 else {
phairero 36:906323851366 187 ms[0]->drive(0.0);
phairero 36:906323851366 188 ms[1]->drive(0.0);
phairero 36:906323851366 189 ms[2]->drive(0.0);
phairero 36:906323851366 190 ms[3]->drive(0.0);
phairero 36:906323851366 191 ms[4]->drive(0.0);
phairero 36:906323851366 192 ms[5]->drive(0.0);
phairero 36:906323851366 193 }
phairero 36:906323851366 194
phairero 36:906323851366 195 //pc.printf("%f", speed);
phairero 32:9db688917ca3 196 //info enkoodritelt
phairero 36:906323851366 197 /*
sim642 29:f905e66e1a6f 198 for (int i = 0; i < MOTORS; i++)
sim642 29:f905e66e1a6f 199 pc.printf("%ld ", ms[i]->encoder.getCount());
sim642 29:f905e66e1a6f 200 pc.printf("\n");
phairero 36:906323851366 201 */
sim642 37:8021b3ce241a 202
phairero 32:9db688917ca3 203 //info IRidelt
sim642 37:8021b3ce241a 204 /*for (int i = 0; i < MOTORS; i++)
phairero 32:9db688917ca3 205 pc.printf("%d ", ss[i].read());
phairero 32:9db688917ca3 206 pc.printf("\n");
sim642 37:8021b3ce241a 207 wait(1.0f);*/
sim642 37:8021b3ce241a 208
sim642 28:fd21faad6dd8 209 //pc.printf("%d", active);
phairero 31:8d8d31b6d8cd 210 //pc.scanf("%f", &turn);
sim642 23:d844cc906b66 211 //m.rotate(turn, 0.2);
phairero 31:8d8d31b6d8cd 212 //pc.scanf("%f %f", &rot, &speed);
sim642 28:fd21faad6dd8 213 //m1.rotate(rot, speed);
sim642 23:d844cc906b66 214 //m.drive(speed);
phairero 31:8d8d31b6d8cd 215 //pc.printf("%f %f\n", s.getTurnSpeed(), m.getSetSpeed());
phairero 31:8d8d31b6d8cd 216 //pc.printf("%f %f\n", m.s, m.getSetSpeed());
phairero 31:8d8d31b6d8cd 217 //pc.printf("%ld %f %f\n", m1.getEncoder().getCount(), m1.getEncoder().getTurn(), m1.getSetTurn());
phairero 31:8d8d31b6d8cd 218 //pc.printf("%f %f\n", m.getEncoder().getTurn(), m.getSetTurn());
phairero 36:906323851366 219
phairero 0:0b7259fdb68a 220 }
phairero 26:c865244ca3cf 221 }