Rauno U
/
Miisu
Six crescent shaped legs
main.cpp@26:c865244ca3cf, 2016-05-12 (annotated)
- Committer:
- phairero
- Date:
- Thu May 12 17:00:10 2016 +0000
- Revision:
- 26:c865244ca3cf
- Parent:
- 25:a8bb69e99d6b
- Child:
- 27:24a9ac72fe92
All motors pinout
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 | 26:c865244ca3cf | 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 | 19:f21eb090a830 | 13 | SyncGroup sync; |
phairero | 26:c865244ca3cf | 14 | |
phairero | 26:c865244ca3cf | 15 | // 1 |
phairero | 26:c865244ca3cf | 16 | MotorData m1Data = {PB_0, PC_0, PC_3}; //PWM, Dir1, Dir2 |
phairero | 26:c865244ca3cf | 17 | EncoderData enc1Data = {PC_1, PA_4, 102.083 * 64}; //EncA, encB // https://www.pololu.com/product/2826 |
phairero | 26:c865244ca3cf | 18 | EncoderMotor m1(m1Data, enc1Data, speedPIDData, turnPIDData, NULL); |
phairero | 26:c865244ca3cf | 19 | |
phairero | 26:c865244ca3cf | 20 | // 2 |
phairero | 26:c865244ca3cf | 21 | MotorData m2Data = {PB_7, PC_14, PC_13}; //PB7 = fault |
phairero | 26:c865244ca3cf | 22 | EncoderData enc2Data = {PC_15, PH_0, 102.083 * 64}; |
phairero | 26:c865244ca3cf | 23 | EncoderMotor m2(m2Data, enc2Data, speedPIDData, turnPIDData, NULL); |
phairero | 26:c865244ca3cf | 24 | |
phairero | 26:c865244ca3cf | 25 | // 3 |
phairero | 26:c865244ca3cf | 26 | MotorData m3Data = {PA_15, PC_11, PC_10}; |
phairero | 26:c865244ca3cf | 27 | EncoderData enc3Data = {PC_12, PA_13, 102.083 * 64}; |
phairero | 26:c865244ca3cf | 28 | EncoderMotor m3(m3Data, enc3Data, speedPIDData, turnPIDData, NULL); |
sim642 | 19:f21eb090a830 | 29 | |
phairero | 26:c865244ca3cf | 30 | // 4 |
phairero | 26:c865244ca3cf | 31 | MotorData m4Data = {PB_8, PC_6, PC_9}; |
phairero | 26:c865244ca3cf | 32 | EncoderData enc4Data = {PC_5, PA_12, 102.083 * 64}; |
phairero | 26:c865244ca3cf | 33 | EncoderMotor m4(m4Data, enc4Data, speedPIDData, turnPIDData, NULL); |
sim642 | 13:dc5f5f19e4e8 | 34 | |
phairero | 26:c865244ca3cf | 35 | // 5 |
phairero | 26:c865244ca3cf | 36 | MotorData m5Data = {PB_15, PB_1, PB_2}; |
phairero | 26:c865244ca3cf | 37 | EncoderData enc5Data = {PB_14, PB_13, 102.083 * 64}; |
phairero | 26:c865244ca3cf | 38 | EncoderMotor m5(m5Data, enc5Data, speedPIDData, turnPIDData, NULL); |
phairero | 26:c865244ca3cf | 39 | |
phairero | 26:c865244ca3cf | 40 | // 6 |
phairero | 26:c865244ca3cf | 41 | //MotorData m6Data = {PB_3, PA_10, PA_2}; //PA_2 = TX; PA_3 (m6-fault) = RX |
phairero | 26:c865244ca3cf | 42 | //EncoderData enc6Data = {PB_5, PB_4, 102.083 * 64}; |
phairero | 26:c865244ca3cf | 43 | //EncoderMotor m6(m6Data, enc6Data, speedPIDData, turnPIDData, NULL); |
phairero | 26:c865244ca3cf | 44 | |
phairero | 26:c865244ca3cf | 45 | //const int mitu = 5; |
phairero | 26:c865244ca3cf | 46 | |
phairero | 26:c865244ca3cf | 47 | //EncoderMotor m[mitu] = {m1,m2,m3,m4,m5};//,m6}; |
sim642 | 23:d844cc906b66 | 48 | |
sim642 | 23:d844cc906b66 | 49 | //SpeedEncoder s(encData); |
sim642 | 8:c3af2a1c206c | 50 | |
sim642 | 4:4d54794b99a5 | 51 | //PIDController ec(0.2, 0.1, 0.01); |
sim642 | 13:dc5f5f19e4e8 | 52 | // PIDController ec(0.3, 2.0, 0.02); |
sim642 | 4:4d54794b99a5 | 53 | //PIDController ec(0.75, 2.0, 0.015); |
sim642 | 5:7f800f61cb13 | 54 | //PIDController ec(0.8, 1.5, 0.017); |
sim642 | 13:dc5f5f19e4e8 | 55 | // PIDController ecRot(5.0, 0.1, 0.04); |
sim642 | 13:dc5f5f19e4e8 | 56 | //EncoderMotor m(mData, encData, ec, ecRot); |
sim642 | 4:4d54794b99a5 | 57 | //Motor m(PB_0, PC_1, PC_0); |
phairero | 0:0b7259fdb68a | 58 | |
sim642 | 2:cf0147952fb9 | 59 | void rise() |
sim642 | 2:cf0147952fb9 | 60 | { |
sim642 | 24:fb1827be6f7e | 61 | //pc.printf("rise\n"); |
phairero | 26:c865244ca3cf | 62 | m1.drive(0); |
sim642 | 2:cf0147952fb9 | 63 | } |
phairero | 0:0b7259fdb68a | 64 | |
sim642 | 2:cf0147952fb9 | 65 | void fall() |
sim642 | 2:cf0147952fb9 | 66 | { |
sim642 | 24:fb1827be6f7e | 67 | //pc.printf("fall\n"); |
phairero | 26:c865244ca3cf | 68 | m1.drive(0.25); |
sim642 | 2:cf0147952fb9 | 69 | } |
phairero | 1:8b0322a353f4 | 70 | |
sim642 | 2:cf0147952fb9 | 71 | int main() |
sim642 | 3:2235787e78c4 | 72 | { |
phairero | 26:c865244ca3cf | 73 | printf("MAIN\n"); |
sim642 | 3:2235787e78c4 | 74 | bt.rise(&rise); |
sim642 | 3:2235787e78c4 | 75 | bt.fall(&fall); |
phairero | 26:c865244ca3cf | 76 | m1.setup(); |
sim642 | 5:7f800f61cb13 | 77 | |
sim642 | 25:a8bb69e99d6b | 78 | float rot, speed; |
sim642 | 25:a8bb69e99d6b | 79 | //m.rotate(1.f, 0.5); |
sim642 | 25:a8bb69e99d6b | 80 | |
sim642 | 25:a8bb69e99d6b | 81 | //sync.update(NULL, 0.5); |
sim642 | 25:a8bb69e99d6b | 82 | |
sim642 | 2:cf0147952fb9 | 83 | while(1) |
sim642 | 2:cf0147952fb9 | 84 | { |
sim642 | 22:bfc79c6ea2fd | 85 | //scanf("%f", &turn); |
sim642 | 23:d844cc906b66 | 86 | //m.rotate(turn, 0.2); |
sim642 | 25:a8bb69e99d6b | 87 | scanf("%f %f", &rot, &speed); |
phairero | 26:c865244ca3cf | 88 | m1.rotate(rot, speed); |
sim642 | 23:d844cc906b66 | 89 | //m.drive(speed); |
sim642 | 23:d844cc906b66 | 90 | //printf("%f %f\n", s.getTurnSpeed(), m.getSetSpeed()); |
sim642 | 23:d844cc906b66 | 91 | //printf("%f %f\n", m.s, m.getSetSpeed()); |
phairero | 26:c865244ca3cf | 92 | printf("%ld %f %f\n", m1.getEncoder().getCount(), m1.getEncoder().getTurn(), m1.getSetTurn()); |
sim642 | 23:d844cc906b66 | 93 | //printf("%f %f\n", m.getEncoder().getTurn(), m.getSetTurn()); |
sim642 | 23:d844cc906b66 | 94 | wait(1.f / 60); |
phairero | 0:0b7259fdb68a | 95 | } |
phairero | 26:c865244ca3cf | 96 | } |