Rauno U
/
Miisu
Six crescent shaped legs
main.cpp@43:0627a2245b9d, 2016-06-16 (annotated)
- Committer:
- phairero
- Date:
- Thu Jun 16 16:56:25 2016 +0000
- Revision:
- 43:0627a2245b9d
- Parent:
- 42:7fa713d5d1af
- Child:
- 44:c2acf8d5e191
Walks in both directions with speed from RF (in air)
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 | 32:9db688917ca3 | 57 | DigitalIn ss[MOTORS] = {s1, s2, s3, s4, s5, s6}; |
sim642 | 35:bce9ac485178 | 58 | |
phairero | 43:0627a2245b9d | 59 | float volatile speed; |
phairero | 43:0627a2245b9d | 60 | |
phairero | 43:0627a2245b9d | 61 | int volatile dir; |
phairero | 43:0627a2245b9d | 62 | |
phairero | 43:0627a2245b9d | 63 | int volatile turn; |
phairero | 43:0627a2245b9d | 64 | |
sim642 | 27:24a9ac72fe92 | 65 | Ticker ticker; |
sim642 | 27:24a9ac72fe92 | 66 | |
sim642 | 2:cf0147952fb9 | 67 | void rise() |
sim642 | 2:cf0147952fb9 | 68 | { |
sim642 | 24:fb1827be6f7e | 69 | //pc.printf("rise\n"); |
sim642 | 28:fd21faad6dd8 | 70 | //m1.drive(0); |
phairero | 36:906323851366 | 71 | //mod = mod*(-1); |
sim642 | 2:cf0147952fb9 | 72 | } |
phairero | 0:0b7259fdb68a | 73 | |
sim642 | 2:cf0147952fb9 | 74 | void fall() |
sim642 | 2:cf0147952fb9 | 75 | { |
sim642 | 24:fb1827be6f7e | 76 | //pc.printf("fall\n"); |
sim642 | 28:fd21faad6dd8 | 77 | //m1.drive(0.25); |
sim642 | 2:cf0147952fb9 | 78 | } |
phairero | 1:8b0322a353f4 | 79 | |
sim642 | 27:24a9ac72fe92 | 80 | void tick() |
sim642 | 27:24a9ac72fe92 | 81 | { |
sim642 | 28:fd21faad6dd8 | 82 | for (int i = 0; i < MOTORS; i++) |
sim642 | 28:fd21faad6dd8 | 83 | ms[i]->tick(); |
sim642 | 27:24a9ac72fe92 | 84 | } |
sim642 | 27:24a9ac72fe92 | 85 | |
sim642 | 27:24a9ac72fe92 | 86 | const float tickTime = 1.f / 60; |
sim642 | 27:24a9ac72fe92 | 87 | |
sim642 | 37:8021b3ce241a | 88 | void waitAllRotate() |
sim642 | 37:8021b3ce241a | 89 | { |
sim642 | 37:8021b3ce241a | 90 | tick(); |
sim642 | 37:8021b3ce241a | 91 | |
sim642 | 37:8021b3ce241a | 92 | float sum; |
sim642 | 37:8021b3ce241a | 93 | do { |
sim642 | 37:8021b3ce241a | 94 | sum = 0; |
sim642 | 37:8021b3ce241a | 95 | for (int i = 0; i < MOTORS; i++) |
sim642 | 37:8021b3ce241a | 96 | sum += abs(ms[i]->errorTurn); |
phairero | 43:0627a2245b9d | 97 | pc.printf(""); |
phairero | 43:0627a2245b9d | 98 | //pc.printf("%f\n",sum); |
sim642 | 37:8021b3ce241a | 99 | } |
sim642 | 37:8021b3ce241a | 100 | while(sum > 0.05f); |
sim642 | 37:8021b3ce241a | 101 | |
sim642 | 37:8021b3ce241a | 102 | wait(0.1f); |
sim642 | 37:8021b3ce241a | 103 | } |
sim642 | 37:8021b3ce241a | 104 | |
sim642 | 42:7fa713d5d1af | 105 | void calibrateLegs() |
sim642 | 42:7fa713d5d1af | 106 | { |
sim642 | 37:8021b3ce241a | 107 | for (int i = 0; i < MOTORS; i++) |
sim642 | 37:8021b3ce241a | 108 | { |
sim642 | 40:01a97bc4ef7a | 109 | if (i != 3) |
sim642 | 39:e35a99801ec1 | 110 | ms[i]->drive(0.2f); |
sim642 | 40:01a97bc4ef7a | 111 | } |
sim642 | 40:01a97bc4ef7a | 112 | |
sim642 | 40:01a97bc4ef7a | 113 | bool done; |
sim642 | 40:01a97bc4ef7a | 114 | do |
sim642 | 40:01a97bc4ef7a | 115 | { |
sim642 | 40:01a97bc4ef7a | 116 | done = true; |
sim642 | 40:01a97bc4ef7a | 117 | for (int i = 0; i < MOTORS; i++) |
sim642 | 40:01a97bc4ef7a | 118 | { |
sim642 | 40:01a97bc4ef7a | 119 | if (i != 3) |
sim642 | 40:01a97bc4ef7a | 120 | { |
sim642 | 40:01a97bc4ef7a | 121 | if (ss[i].read()) |
sim642 | 40:01a97bc4ef7a | 122 | done = false; |
sim642 | 40:01a97bc4ef7a | 123 | else |
sim642 | 40:01a97bc4ef7a | 124 | ms[i]->drive(0.f); |
sim642 | 40:01a97bc4ef7a | 125 | } |
sim642 | 37:8021b3ce241a | 126 | } |
sim642 | 40:01a97bc4ef7a | 127 | } |
sim642 | 40:01a97bc4ef7a | 128 | while (!done); |
sim642 | 40:01a97bc4ef7a | 129 | |
sim642 | 40:01a97bc4ef7a | 130 | for (int i = 0; i < MOTORS; i++) |
sim642 | 37:8021b3ce241a | 131 | ms[i]->getEncoder().reset(); |
sim642 | 42:7fa713d5d1af | 132 | |
phairero | 43:0627a2245b9d | 133 | pc.printf("done\n"); |
sim642 | 42:7fa713d5d1af | 134 | } |
sim642 | 42:7fa713d5d1af | 135 | |
sim642 | 42:7fa713d5d1af | 136 | void standUp() |
sim642 | 42:7fa713d5d1af | 137 | { |
sim642 | 39:e35a99801ec1 | 138 | ms[0]->rotate(0.5f + 0.125f, 0.75f); |
sim642 | 39:e35a99801ec1 | 139 | ms[1]->rotate(0.5f - 0.125f, 0.75f); |
sim642 | 38:b03a5bf9ac7b | 140 | ms[2]->rotate(0.5f + 0.125f, 0.75f); |
sim642 | 38:b03a5bf9ac7b | 141 | ms[3]->rotate(0.5f - 0.125f, 0.75f); |
sim642 | 38:b03a5bf9ac7b | 142 | ms[4]->rotate(0.5f + 0.125f, 0.75f); |
sim642 | 39:e35a99801ec1 | 143 | ms[5]->rotate(0.5f - 0.125f, 0.75f); |
sim642 | 37:8021b3ce241a | 144 | waitAllRotate(); |
sim642 | 42:7fa713d5d1af | 145 | } |
sim642 | 42:7fa713d5d1af | 146 | |
phairero | 43:0627a2245b9d | 147 | void moveOne() |
phairero | 43:0627a2245b9d | 148 | { |
phairero | 43:0627a2245b9d | 149 | ms[0]->rotate(dir*0.75f,speed); |
phairero | 43:0627a2245b9d | 150 | ms[1]->rotate(dir*0.25f,speed/3); |
phairero | 43:0627a2245b9d | 151 | ms[2]->rotate(dir*0.75f,speed); |
phairero | 43:0627a2245b9d | 152 | ms[3]->rotate(dir*0.25f,speed/3); |
phairero | 43:0627a2245b9d | 153 | ms[4]->rotate(dir*0.75f,speed); |
phairero | 43:0627a2245b9d | 154 | ms[5]->rotate(dir*0.25f,speed/3); |
phairero | 43:0627a2245b9d | 155 | waitAllRotate(); |
phairero | 43:0627a2245b9d | 156 | } |
phairero | 43:0627a2245b9d | 157 | |
phairero | 43:0627a2245b9d | 158 | void moveTwo(){ |
phairero | 43:0627a2245b9d | 159 | ms[0]->rotate(dir*0.25f,speed/3); |
phairero | 43:0627a2245b9d | 160 | ms[1]->rotate(dir*0.75f,speed); |
phairero | 43:0627a2245b9d | 161 | ms[2]->rotate(dir*0.25f,speed/3); |
phairero | 43:0627a2245b9d | 162 | ms[3]->rotate(dir*0.75f,speed); |
phairero | 43:0627a2245b9d | 163 | ms[4]->rotate(dir*0.25f,speed/3); |
phairero | 43:0627a2245b9d | 164 | ms[5]->rotate(dir*0.75f,speed); |
phairero | 43:0627a2245b9d | 165 | waitAllRotate(); |
phairero | 43:0627a2245b9d | 166 | } |
phairero | 43:0627a2245b9d | 167 | |
phairero | 43:0627a2245b9d | 168 | |
sim642 | 42:7fa713d5d1af | 169 | int main() |
sim642 | 42:7fa713d5d1af | 170 | { |
sim642 | 42:7fa713d5d1af | 171 | pc.printf("MAIN\n"); |
sim642 | 42:7fa713d5d1af | 172 | bt.rise(&rise); |
sim642 | 42:7fa713d5d1af | 173 | bt.fall(&fall); |
sim642 | 42:7fa713d5d1af | 174 | |
sim642 | 42:7fa713d5d1af | 175 | //ms[active]->drive(0.25); |
sim642 | 42:7fa713d5d1af | 176 | |
sim642 | 42:7fa713d5d1af | 177 | ticker.attach(&tick, tickTime); |
sim642 | 42:7fa713d5d1af | 178 | |
sim642 | 42:7fa713d5d1af | 179 | //unsigned char volatile rfget; |
sim642 | 42:7fa713d5d1af | 180 | |
sim642 | 42:7fa713d5d1af | 181 | calibrateLegs(); |
sim642 | 42:7fa713d5d1af | 182 | standUp(); |
sim642 | 37:8021b3ce241a | 183 | |
phairero | 43:0627a2245b9d | 184 | //float volatile sum; |
phairero | 36:906323851366 | 185 | |
phairero | 43:0627a2245b9d | 186 | speed = 0.0f; |
phairero | 43:0627a2245b9d | 187 | dir = 1; |
phairero | 43:0627a2245b9d | 188 | turn = 0; |
phairero | 36:906323851366 | 189 | |
phairero | 43:0627a2245b9d | 190 | pc.printf("start\n"); |
sim642 | 2:cf0147952fb9 | 191 | while(1) |
phairero | 36:906323851366 | 192 | { |
phairero | 36:906323851366 | 193 | |
phairero | 43:0627a2245b9d | 194 | //speed = 1.5f; |
sim642 | 37:8021b3ce241a | 195 | //speed = 0; |
phairero | 36:906323851366 | 196 | //int oldactive = active; |
phairero | 43:0627a2245b9d | 197 | if (pc.readable()){ |
phairero | 43:0627a2245b9d | 198 | pc.scanf("%f %d %d", &speed, &dir, &turn); |
phairero | 43:0627a2245b9d | 199 | if (speed >= 10){ |
phairero | 43:0627a2245b9d | 200 | speed = speed - (((int)(speed/10))*10); |
phairero | 43:0627a2245b9d | 201 | } |
phairero | 43:0627a2245b9d | 202 | } |
phairero | 43:0627a2245b9d | 203 | pc.printf("%f %d%d\n",speed,dir,turn); |
phairero | 43:0627a2245b9d | 204 | |
phairero | 36:906323851366 | 205 | if (speed != 0){ |
phairero | 43:0627a2245b9d | 206 | //for (int i = 0; i < MOTORS; i++) |
phairero | 43:0627a2245b9d | 207 | // pc.printf("%ld ", ms[i]->encoder.getCount()); |
phairero | 43:0627a2245b9d | 208 | //pc.printf("\n"); |
phairero | 43:0627a2245b9d | 209 | |
phairero | 43:0627a2245b9d | 210 | //speed = (rfget*0.3f); //+ ((speed < 0) ? -1 : 1) * mod; |
sim642 | 37:8021b3ce241a | 211 | |
phairero | 43:0627a2245b9d | 212 | if (dir == 1){ |
phairero | 43:0627a2245b9d | 213 | moveOne(); |
phairero | 43:0627a2245b9d | 214 | moveTwo(); |
phairero | 43:0627a2245b9d | 215 | } |
phairero | 43:0627a2245b9d | 216 | else { |
phairero | 43:0627a2245b9d | 217 | moveTwo(); |
phairero | 43:0627a2245b9d | 218 | moveOne(); |
phairero | 43:0627a2245b9d | 219 | } |
phairero | 43:0627a2245b9d | 220 | |
phairero | 43:0627a2245b9d | 221 | /*ms[0]->rotate(dir*0.75f,speed); |
phairero | 43:0627a2245b9d | 222 | ms[1]->rotate(dir*0.25f,speed/3); |
phairero | 43:0627a2245b9d | 223 | ms[2]->rotate(dir*0.75f,speed); |
phairero | 43:0627a2245b9d | 224 | ms[3]->rotate(dir*0.25f,speed/3); |
phairero | 43:0627a2245b9d | 225 | ms[4]->rotate(dir*0.75f,speed); |
phairero | 43:0627a2245b9d | 226 | ms[5]->rotate(dir*0.25f,speed/3); |
sim642 | 37:8021b3ce241a | 227 | waitAllRotate(); |
sim642 | 37:8021b3ce241a | 228 | |
phairero | 43:0627a2245b9d | 229 | ms[0]->rotate(dir*0.25f,speed/3); |
phairero | 43:0627a2245b9d | 230 | ms[1]->rotate(dir*0.75f,speed); |
phairero | 43:0627a2245b9d | 231 | ms[2]->rotate(dir*0.25f,speed/3); |
phairero | 43:0627a2245b9d | 232 | ms[3]->rotate(dir*0.75f,speed); |
phairero | 43:0627a2245b9d | 233 | ms[4]->rotate(dir*0.25f,speed/3); |
phairero | 43:0627a2245b9d | 234 | ms[5]->rotate(dir*0.75f,speed); |
phairero | 43:0627a2245b9d | 235 | waitAllRotate();*/ |
phairero | 36:906323851366 | 236 | } |
phairero | 36:906323851366 | 237 | else { |
phairero | 36:906323851366 | 238 | ms[0]->drive(0.0); |
phairero | 36:906323851366 | 239 | ms[1]->drive(0.0); |
phairero | 36:906323851366 | 240 | ms[2]->drive(0.0); |
phairero | 36:906323851366 | 241 | ms[3]->drive(0.0); |
phairero | 36:906323851366 | 242 | ms[4]->drive(0.0); |
phairero | 36:906323851366 | 243 | ms[5]->drive(0.0); |
phairero | 36:906323851366 | 244 | } |
phairero | 36:906323851366 | 245 | |
phairero | 36:906323851366 | 246 | //pc.printf("%f", speed); |
phairero | 32:9db688917ca3 | 247 | //info enkoodritelt |
phairero | 36:906323851366 | 248 | /* |
sim642 | 29:f905e66e1a6f | 249 | for (int i = 0; i < MOTORS; i++) |
sim642 | 29:f905e66e1a6f | 250 | pc.printf("%ld ", ms[i]->encoder.getCount()); |
sim642 | 29:f905e66e1a6f | 251 | pc.printf("\n"); |
phairero | 36:906323851366 | 252 | */ |
sim642 | 37:8021b3ce241a | 253 | |
phairero | 32:9db688917ca3 | 254 | //info IRidelt |
sim642 | 37:8021b3ce241a | 255 | /*for (int i = 0; i < MOTORS; i++) |
phairero | 32:9db688917ca3 | 256 | pc.printf("%d ", ss[i].read()); |
phairero | 32:9db688917ca3 | 257 | pc.printf("\n"); |
sim642 | 37:8021b3ce241a | 258 | wait(1.0f);*/ |
sim642 | 37:8021b3ce241a | 259 | |
sim642 | 28:fd21faad6dd8 | 260 | //pc.printf("%d", active); |
phairero | 31:8d8d31b6d8cd | 261 | //pc.scanf("%f", &turn); |
sim642 | 23:d844cc906b66 | 262 | //m.rotate(turn, 0.2); |
phairero | 31:8d8d31b6d8cd | 263 | //pc.scanf("%f %f", &rot, &speed); |
sim642 | 28:fd21faad6dd8 | 264 | //m1.rotate(rot, speed); |
sim642 | 23:d844cc906b66 | 265 | //m.drive(speed); |
phairero | 31:8d8d31b6d8cd | 266 | //pc.printf("%f %f\n", s.getTurnSpeed(), m.getSetSpeed()); |
phairero | 31:8d8d31b6d8cd | 267 | //pc.printf("%f %f\n", m.s, m.getSetSpeed()); |
phairero | 31:8d8d31b6d8cd | 268 | //pc.printf("%ld %f %f\n", m1.getEncoder().getCount(), m1.getEncoder().getTurn(), m1.getSetTurn()); |
phairero | 31:8d8d31b6d8cd | 269 | //pc.printf("%f %f\n", m.getEncoder().getTurn(), m.getSetTurn()); |
phairero | 36:906323851366 | 270 | |
phairero | 0:0b7259fdb68a | 271 | } |
phairero | 26:c865244ca3cf | 272 | } |