Rauno U
/
Miisu
Six crescent shaped legs
main.cpp@34:30391b6c26fc, 2016-06-02 (annotated)
- Committer:
- phairero
- Date:
- Thu Jun 02 14:15:10 2016 +0000
- Revision:
- 34:30391b6c26fc
- Parent:
- 33:ad2ebe22f8dd
- Child:
- 35:bce9ac485178
Motor-by-motor speed from a controller over RF
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" |
sim642 | 19:f21eb090a830 | 3 | #include "SyncGroup.hpp" |
phairero | 0:0b7259fdb68a | 4 | |
phairero | 0:0b7259fdb68a | 5 | InterruptIn bt(USER_BUTTON); |
phairero | 30:476ab400d765 | 6 | Serial pc(PA_0, PA_1); //RF |
phairero | 30:476ab400d765 | 7 | //Serial pc(SERIAL_TX, SERIAL_RX); |
phairero | 26:c865244ca3cf | 8 | //Serial pc(USBTX, USBRX); |
sim642 | 2:cf0147952fb9 | 9 | |
sim642 | 23:d844cc906b66 | 10 | //PIDData speedPIDData = {0.3f, 2.0f, 0.02f}; |
sim642 | 23:d844cc906b66 | 11 | //PIDData turnPIDData = {5.0f, 0.1f, 0.04f}; |
sim642 | 23:d844cc906b66 | 12 | PIDData speedPIDData = {0.5f, 0.0f, 0.0f}; |
phairero | 26:c865244ca3cf | 13 | PIDData turnPIDData = {30.0f, 0.01f, 1.0f}; |
sim642 | 19:f21eb090a830 | 14 | SyncGroup sync; |
sim642 | 27:24a9ac72fe92 | 15 | |
sim642 | 27:24a9ac72fe92 | 16 | /* |
sim642 | 27:24a9ac72fe92 | 17 | PWM timer channel |
sim642 | 27:24a9ac72fe92 | 18 | M1 1 2N |
sim642 | 27:24a9ac72fe92 | 19 | M2 4 2 |
sim642 | 27:24a9ac72fe92 | 20 | M3 2 1 |
sim642 | 27:24a9ac72fe92 | 21 | M4 4 4 |
sim642 | 27:24a9ac72fe92 | 22 | M5 1 3N |
sim642 | 27:24a9ac72fe92 | 23 | M6 2 2 |
sim642 | 27:24a9ac72fe92 | 24 | */ |
phairero | 26:c865244ca3cf | 25 | |
phairero | 26:c865244ca3cf | 26 | // 1 |
phairero | 26:c865244ca3cf | 27 | MotorData m1Data = {PB_0, PC_0, PC_3}; //PWM, Dir1, Dir2 |
phairero | 26:c865244ca3cf | 28 | EncoderData enc1Data = {PC_1, PA_4, 102.083 * 64}; //EncA, encB // https://www.pololu.com/product/2826 |
phairero | 26:c865244ca3cf | 29 | EncoderMotor m1(m1Data, enc1Data, speedPIDData, turnPIDData, NULL); |
phairero | 32:9db688917ca3 | 30 | DigitalIn s1(PA_8); |
phairero | 26:c865244ca3cf | 31 | // 2 |
phairero | 33:ad2ebe22f8dd | 32 | MotorData m2Data = {PB_7, PC_14, PA_6}; //PB7 = fault dir2 oli enne PC13 |
phairero | 26:c865244ca3cf | 33 | EncoderData enc2Data = {PC_15, PH_0, 102.083 * 64}; |
phairero | 26:c865244ca3cf | 34 | EncoderMotor m2(m2Data, enc2Data, speedPIDData, turnPIDData, NULL); |
phairero | 32:9db688917ca3 | 35 | DigitalIn s2(PH_1); |
phairero | 26:c865244ca3cf | 36 | // 3 |
phairero | 26:c865244ca3cf | 37 | MotorData m3Data = {PA_15, PC_11, PC_10}; |
phairero | 33:ad2ebe22f8dd | 38 | EncoderData enc3Data = {PC_12, PA_9, 102.083 * 64}; //B oli varem PA_13 |
phairero | 26:c865244ca3cf | 39 | EncoderMotor m3(m3Data, enc3Data, speedPIDData, turnPIDData, NULL); |
phairero | 32:9db688917ca3 | 40 | DigitalIn s3(PA_14); |
phairero | 26:c865244ca3cf | 41 | // 4 |
phairero | 26:c865244ca3cf | 42 | MotorData m4Data = {PB_8, PC_6, PC_9}; |
phairero | 26:c865244ca3cf | 43 | EncoderData enc4Data = {PC_5, PA_12, 102.083 * 64}; |
phairero | 26:c865244ca3cf | 44 | EncoderMotor m4(m4Data, enc4Data, speedPIDData, turnPIDData, NULL); |
phairero | 32:9db688917ca3 | 45 | DigitalIn s4(PA_11); |
phairero | 26:c865244ca3cf | 46 | // 5 |
phairero | 26:c865244ca3cf | 47 | MotorData m5Data = {PB_15, PB_1, PB_2}; |
phairero | 33:ad2ebe22f8dd | 48 | EncoderData enc5Data = {PC_7, PB_6, 102.083 * 64}; //B oli varem PB_13, A oli varem PB_14 |
phairero | 26:c865244ca3cf | 49 | EncoderMotor m5(m5Data, enc5Data, speedPIDData, turnPIDData, NULL); |
phairero | 32:9db688917ca3 | 50 | DigitalIn s5(PC_4); |
phairero | 26:c865244ca3cf | 51 | // 6 |
phairero | 33:ad2ebe22f8dd | 52 | MotorData m6Data = {PB_3, PA_10, PA_7}; //PA_2 = TX; PA_3 (m6-fault) = RX DIR2 oli enne PA2 |
phairero | 30:476ab400d765 | 53 | EncoderData enc6Data = {PB_5, PB_4, 102.083 * 64}; |
phairero | 30:476ab400d765 | 54 | EncoderMotor m6(m6Data, enc6Data, speedPIDData, turnPIDData, NULL); |
phairero | 32:9db688917ca3 | 55 | DigitalIn s6(PB_10); |
phairero | 30:476ab400d765 | 56 | const int MOTORS = 6; |
phairero | 30:476ab400d765 | 57 | EncoderMotor* ms[MOTORS] = {&m1, &m2, &m3, &m4, &m5, &m6}; |
sim642 | 28:fd21faad6dd8 | 58 | volatile int active = 0; |
phairero | 32:9db688917ca3 | 59 | DigitalIn ss[MOTORS] = {s1, s2, s3, s4, s5, s6}; |
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); |
sim642 | 2:cf0147952fb9 | 66 | } |
phairero | 0:0b7259fdb68a | 67 | |
sim642 | 2:cf0147952fb9 | 68 | void fall() |
sim642 | 2:cf0147952fb9 | 69 | { |
sim642 | 24:fb1827be6f7e | 70 | //pc.printf("fall\n"); |
sim642 | 28:fd21faad6dd8 | 71 | //m1.drive(0.25); |
sim642 | 2:cf0147952fb9 | 72 | } |
phairero | 1:8b0322a353f4 | 73 | |
sim642 | 27:24a9ac72fe92 | 74 | void tick() |
sim642 | 27:24a9ac72fe92 | 75 | { |
sim642 | 28:fd21faad6dd8 | 76 | for (int i = 0; i < MOTORS; i++) |
sim642 | 28:fd21faad6dd8 | 77 | ms[i]->tick(); |
sim642 | 27:24a9ac72fe92 | 78 | } |
sim642 | 27:24a9ac72fe92 | 79 | |
sim642 | 27:24a9ac72fe92 | 80 | const float tickTime = 1.f / 60; |
sim642 | 27:24a9ac72fe92 | 81 | |
sim642 | 2:cf0147952fb9 | 82 | int main() |
sim642 | 3:2235787e78c4 | 83 | { |
phairero | 31:8d8d31b6d8cd | 84 | pc.printf("MAIN\n"); |
sim642 | 3:2235787e78c4 | 85 | bt.rise(&rise); |
sim642 | 3:2235787e78c4 | 86 | bt.fall(&fall); |
sim642 | 27:24a9ac72fe92 | 87 | |
sim642 | 28:fd21faad6dd8 | 88 | //ms[active]->drive(0.25); |
sim642 | 28:fd21faad6dd8 | 89 | |
sim642 | 27:24a9ac72fe92 | 90 | ticker.attach(&tick, tickTime); |
sim642 | 5:7f800f61cb13 | 91 | |
phairero | 33:ad2ebe22f8dd | 92 | float volatile rot, speed; |
phairero | 33:ad2ebe22f8dd | 93 | |
phairero | 34:30391b6c26fc | 94 | speed = 0.0; |
sim642 | 25:a8bb69e99d6b | 95 | |
sim642 | 2:cf0147952fb9 | 96 | while(1) |
phairero | 34:30391b6c26fc | 97 | {/* |
phairero | 33:ad2ebe22f8dd | 98 | int oldactive = active; |
phairero | 34:30391b6c26fc | 99 | pc.scanf("%d %f", &active, &speed); |
phairero | 34:30391b6c26fc | 100 | //active--; |
phairero | 34:30391b6c26fc | 101 | if (active != oldactive){ |
phairero | 34:30391b6c26fc | 102 | ms[oldactive]->drive(0); |
phairero | 33:ad2ebe22f8dd | 103 | } |
phairero | 34:30391b6c26fc | 104 | ms[active]->drive(speed); |
phairero | 34:30391b6c26fc | 105 | */ |
phairero | 32:9db688917ca3 | 106 | //info enkoodritelt |
phairero | 34:30391b6c26fc | 107 | |
sim642 | 29:f905e66e1a6f | 108 | for (int i = 0; i < MOTORS; i++) |
sim642 | 29:f905e66e1a6f | 109 | pc.printf("%ld ", ms[i]->encoder.getCount()); |
sim642 | 29:f905e66e1a6f | 110 | pc.printf("\n"); |
phairero | 34:30391b6c26fc | 111 | |
phairero | 33:ad2ebe22f8dd | 112 | /* |
phairero | 32:9db688917ca3 | 113 | //info IRidelt |
phairero | 32:9db688917ca3 | 114 | for (int i = 0; i < MOTORS; i++) |
phairero | 32:9db688917ca3 | 115 | pc.printf("%d ", ss[i].read()); |
phairero | 32:9db688917ca3 | 116 | pc.printf("\n"); |
phairero | 33:ad2ebe22f8dd | 117 | */ |
sim642 | 29:f905e66e1a6f | 118 | |
sim642 | 28:fd21faad6dd8 | 119 | //pc.printf("%d", active); |
phairero | 31:8d8d31b6d8cd | 120 | //pc.scanf("%f", &turn); |
sim642 | 23:d844cc906b66 | 121 | //m.rotate(turn, 0.2); |
phairero | 31:8d8d31b6d8cd | 122 | //pc.scanf("%f %f", &rot, &speed); |
sim642 | 28:fd21faad6dd8 | 123 | //m1.rotate(rot, speed); |
sim642 | 23:d844cc906b66 | 124 | //m.drive(speed); |
phairero | 31:8d8d31b6d8cd | 125 | //pc.printf("%f %f\n", s.getTurnSpeed(), m.getSetSpeed()); |
phairero | 31:8d8d31b6d8cd | 126 | //pc.printf("%f %f\n", m.s, m.getSetSpeed()); |
phairero | 31:8d8d31b6d8cd | 127 | //pc.printf("%ld %f %f\n", m1.getEncoder().getCount(), m1.getEncoder().getTurn(), m1.getSetTurn()); |
phairero | 31:8d8d31b6d8cd | 128 | //pc.printf("%f %f\n", m.getEncoder().getTurn(), m.getSetTurn()); |
sim642 | 28:fd21faad6dd8 | 129 | wait(1.f / 10); |
phairero | 0:0b7259fdb68a | 130 | } |
phairero | 26:c865244ca3cf | 131 | } |