Rauno U
/
Miisu
Six crescent shaped legs
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?
User | Revision | Line number | New 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 | } |