Rauno U / Mbed 2 deprecated Miisu

Six crescent shaped legs

Dependencies:   mbed

main.cpp@31:8d8d31b6d8cd, 2016-05-17 (annotated)

Committer:
phairero
Date:
Tue May 17 17:07:05 2016 +0000
Revision:
31:8d8d31b6d8cd
Parent:
30:476ab400d765
Child:
32:9db688917ca3
There was a 'pc.' missing

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 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 26:c865244ca3cf 30
phairero 26:c865244ca3cf 31 // 2
phairero 26:c865244ca3cf 32 MotorData m2Data = {PB_7, PC_14, PC_13}; //PB7 = fault
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 26:c865244ca3cf 35
phairero 26:c865244ca3cf 36 // 3
phairero 26:c865244ca3cf 37 MotorData m3Data = {PA_15, PC_11, PC_10};
phairero 26:c865244ca3cf 38 EncoderData enc3Data = {PC_12, PA_13, 102.083 * 64};
phairero 26:c865244ca3cf 39 EncoderMotor m3(m3Data, enc3Data, speedPIDData, turnPIDData, NULL);
sim642 19:f21eb090a830 40
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);
sim642 13:dc5f5f19e4e8 45
phairero 26:c865244ca3cf 46 // 5
phairero 26:c865244ca3cf 47 MotorData m5Data = {PB_15, PB_1, PB_2};
phairero 26:c865244ca3cf 48 EncoderData enc5Data = {PB_14, PB_13, 102.083 * 64};
phairero 26:c865244ca3cf 49 EncoderMotor m5(m5Data, enc5Data, speedPIDData, turnPIDData, NULL);
phairero 26:c865244ca3cf 50
phairero 26:c865244ca3cf 51 // 6
phairero 30:476ab400d765 52 MotorData m6Data = {PB_3, PA_10, PA_2}; //PA_2 = TX; PA_3 (m6-fault) = RX
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 26:c865244ca3cf 55
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 0:0b7259fdb68a 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);
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
sim642 25:a8bb69e99d6b 92 float rot, speed;
sim642 25:a8bb69e99d6b 93
sim642 2:cf0147952fb9 94 while(1)
sim642 2:cf0147952fb9 95 {
sim642 29:f905e66e1a6f 96 /*int oldactive = active;
phairero 31:8d8d31b6d8cd 97 pc.scanf("%d", &active);
sim642 28:fd21faad6dd8 98 active--;
sim642 28:fd21faad6dd8 99 ms[oldactive]->drive(0);
sim642 29:f905e66e1a6f 100 ms[active]->drive(0.25);*/
sim642 29:f905e66e1a6f 101
sim642 29:f905e66e1a6f 102 for (int i = 0; i < MOTORS; i++)
sim642 29:f905e66e1a6f 103 pc.printf("%ld ", ms[i]->encoder.getCount());
sim642 29:f905e66e1a6f 104 pc.printf("\n");
sim642 29:f905e66e1a6f 105
sim642 28:fd21faad6dd8 106 //pc.printf("%d", active);
phairero 31:8d8d31b6d8cd 107 //pc.scanf("%f", &turn);
sim642 23:d844cc906b66 108 //m.rotate(turn, 0.2);
phairero 31:8d8d31b6d8cd 109 //pc.scanf("%f %f", &rot, &speed);
sim642 28:fd21faad6dd8 110 //m1.rotate(rot, speed);
sim642 23:d844cc906b66 111 //m.drive(speed);
phairero 31:8d8d31b6d8cd 112 //pc.printf("%f %f\n", s.getTurnSpeed(), m.getSetSpeed());
phairero 31:8d8d31b6d8cd 113 //pc.printf("%f %f\n", m.s, m.getSetSpeed());
phairero 31:8d8d31b6d8cd 114 //pc.printf("%ld %f %f\n", m1.getEncoder().getCount(), m1.getEncoder().getTurn(), m1.getSetTurn());
phairero 31:8d8d31b6d8cd 115 //pc.printf("%f %f\n", m.getEncoder().getTurn(), m.getSetTurn());
sim642 28:fd21faad6dd8 116 wait(1.f / 10);
phairero 0:0b7259fdb68a 117 }
phairero 26:c865244ca3cf 118 }