Six crescent shaped legs

Dependencies:   mbed

Committer:
sim642
Date:
Thu May 12 19:05:05 2016 +0000
Revision:
27:24a9ac72fe92
Parent:
26:c865244ca3cf
Child:
28:fd21faad6dd8
Use global ticker for EncoderMotors

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"
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;
sim642 27:24a9ac72fe92 14
sim642 27:24a9ac72fe92 15 /*
sim642 27:24a9ac72fe92 16 PWM timer channel
sim642 27:24a9ac72fe92 17 M1 1 2N
sim642 27:24a9ac72fe92 18 M2 4 2
sim642 27:24a9ac72fe92 19 M3 2 1
sim642 27:24a9ac72fe92 20 M4 4 4
sim642 27:24a9ac72fe92 21 M5 1 3N
sim642 27:24a9ac72fe92 22 M6 2 2
sim642 27:24a9ac72fe92 23 */
phairero 26:c865244ca3cf 24
phairero 26:c865244ca3cf 25 // 1
phairero 26:c865244ca3cf 26 MotorData m1Data = {PB_0, PC_0, PC_3}; //PWM, Dir1, Dir2
phairero 26:c865244ca3cf 27 EncoderData enc1Data = {PC_1, PA_4, 102.083 * 64}; //EncA, encB // https://www.pololu.com/product/2826
phairero 26:c865244ca3cf 28 EncoderMotor m1(m1Data, enc1Data, speedPIDData, turnPIDData, NULL);
phairero 26:c865244ca3cf 29
phairero 26:c865244ca3cf 30 // 2
phairero 26:c865244ca3cf 31 MotorData m2Data = {PB_7, PC_14, PC_13}; //PB7 = fault
phairero 26:c865244ca3cf 32 EncoderData enc2Data = {PC_15, PH_0, 102.083 * 64};
phairero 26:c865244ca3cf 33 EncoderMotor m2(m2Data, enc2Data, speedPIDData, turnPIDData, NULL);
phairero 26:c865244ca3cf 34
phairero 26:c865244ca3cf 35 // 3
phairero 26:c865244ca3cf 36 MotorData m3Data = {PA_15, PC_11, PC_10};
phairero 26:c865244ca3cf 37 EncoderData enc3Data = {PC_12, PA_13, 102.083 * 64};
phairero 26:c865244ca3cf 38 EncoderMotor m3(m3Data, enc3Data, speedPIDData, turnPIDData, NULL);
sim642 19:f21eb090a830 39
phairero 26:c865244ca3cf 40 // 4
phairero 26:c865244ca3cf 41 MotorData m4Data = {PB_8, PC_6, PC_9};
phairero 26:c865244ca3cf 42 EncoderData enc4Data = {PC_5, PA_12, 102.083 * 64};
phairero 26:c865244ca3cf 43 EncoderMotor m4(m4Data, enc4Data, speedPIDData, turnPIDData, NULL);
sim642 13:dc5f5f19e4e8 44
phairero 26:c865244ca3cf 45 // 5
phairero 26:c865244ca3cf 46 MotorData m5Data = {PB_15, PB_1, PB_2};
phairero 26:c865244ca3cf 47 EncoderData enc5Data = {PB_14, PB_13, 102.083 * 64};
phairero 26:c865244ca3cf 48 EncoderMotor m5(m5Data, enc5Data, speedPIDData, turnPIDData, NULL);
phairero 26:c865244ca3cf 49
phairero 26:c865244ca3cf 50 // 6
phairero 26:c865244ca3cf 51 //MotorData m6Data = {PB_3, PA_10, PA_2}; //PA_2 = TX; PA_3 (m6-fault) = RX
phairero 26:c865244ca3cf 52 //EncoderData enc6Data = {PB_5, PB_4, 102.083 * 64};
phairero 26:c865244ca3cf 53 //EncoderMotor m6(m6Data, enc6Data, speedPIDData, turnPIDData, NULL);
phairero 26:c865244ca3cf 54
phairero 26:c865244ca3cf 55 //const int mitu = 5;
phairero 26:c865244ca3cf 56
phairero 26:c865244ca3cf 57 //EncoderMotor m[mitu] = {m1,m2,m3,m4,m5};//,m6};
sim642 23:d844cc906b66 58
sim642 23:d844cc906b66 59 //SpeedEncoder s(encData);
sim642 8:c3af2a1c206c 60
sim642 4:4d54794b99a5 61 //PIDController ec(0.2, 0.1, 0.01);
sim642 13:dc5f5f19e4e8 62 // PIDController ec(0.3, 2.0, 0.02);
sim642 4:4d54794b99a5 63 //PIDController ec(0.75, 2.0, 0.015);
sim642 5:7f800f61cb13 64 //PIDController ec(0.8, 1.5, 0.017);
sim642 13:dc5f5f19e4e8 65 // PIDController ecRot(5.0, 0.1, 0.04);
sim642 13:dc5f5f19e4e8 66 //EncoderMotor m(mData, encData, ec, ecRot);
sim642 4:4d54794b99a5 67 //Motor m(PB_0, PC_1, PC_0);
phairero 0:0b7259fdb68a 68
sim642 27:24a9ac72fe92 69 Ticker ticker;
sim642 27:24a9ac72fe92 70
sim642 2:cf0147952fb9 71 void rise()
sim642 2:cf0147952fb9 72 {
sim642 24:fb1827be6f7e 73 //pc.printf("rise\n");
phairero 26:c865244ca3cf 74 m1.drive(0);
sim642 2:cf0147952fb9 75 }
phairero 0:0b7259fdb68a 76
sim642 2:cf0147952fb9 77 void fall()
sim642 2:cf0147952fb9 78 {
sim642 24:fb1827be6f7e 79 //pc.printf("fall\n");
phairero 26:c865244ca3cf 80 m1.drive(0.25);
sim642 2:cf0147952fb9 81 }
phairero 1:8b0322a353f4 82
sim642 27:24a9ac72fe92 83 void tick()
sim642 27:24a9ac72fe92 84 {
sim642 27:24a9ac72fe92 85 m1.tick();
sim642 27:24a9ac72fe92 86 m2.tick();
sim642 27:24a9ac72fe92 87 m3.tick();
sim642 27:24a9ac72fe92 88 m4.tick();
sim642 27:24a9ac72fe92 89 m5.tick();
sim642 27:24a9ac72fe92 90 //m6.tick();
sim642 27:24a9ac72fe92 91 }
sim642 27:24a9ac72fe92 92
sim642 27:24a9ac72fe92 93 const float tickTime = 1.f / 60;
sim642 27:24a9ac72fe92 94
sim642 2:cf0147952fb9 95 int main()
sim642 3:2235787e78c4 96 {
phairero 26:c865244ca3cf 97 printf("MAIN\n");
sim642 3:2235787e78c4 98 bt.rise(&rise);
sim642 3:2235787e78c4 99 bt.fall(&fall);
sim642 27:24a9ac72fe92 100
sim642 27:24a9ac72fe92 101 ticker.attach(&tick, tickTime);
sim642 5:7f800f61cb13 102
sim642 25:a8bb69e99d6b 103 float rot, speed;
sim642 25:a8bb69e99d6b 104 //m.rotate(1.f, 0.5);
sim642 25:a8bb69e99d6b 105
sim642 25:a8bb69e99d6b 106 //sync.update(NULL, 0.5);
sim642 25:a8bb69e99d6b 107
sim642 2:cf0147952fb9 108 while(1)
sim642 2:cf0147952fb9 109 {
sim642 22:bfc79c6ea2fd 110 //scanf("%f", &turn);
sim642 23:d844cc906b66 111 //m.rotate(turn, 0.2);
sim642 25:a8bb69e99d6b 112 scanf("%f %f", &rot, &speed);
phairero 26:c865244ca3cf 113 m1.rotate(rot, speed);
sim642 23:d844cc906b66 114 //m.drive(speed);
sim642 23:d844cc906b66 115 //printf("%f %f\n", s.getTurnSpeed(), m.getSetSpeed());
sim642 23:d844cc906b66 116 //printf("%f %f\n", m.s, m.getSetSpeed());
phairero 26:c865244ca3cf 117 printf("%ld %f %f\n", m1.getEncoder().getCount(), m1.getEncoder().getTurn(), m1.getSetTurn());
sim642 23:d844cc906b66 118 //printf("%f %f\n", m.getEncoder().getTurn(), m.getSetTurn());
sim642 23:d844cc906b66 119 wait(1.f / 60);
phairero 0:0b7259fdb68a 120 }
phairero 26:c865244ca3cf 121 }