Rauno U / Mbed 2 deprecated Miisu

Six crescent shaped legs

Dependencies:   mbed

main.cpp@36:906323851366, 2016-06-15 (annotated)

Committer:
phairero
Date:
Wed Jun 15 11:52:08 2016 +0000
Revision:
36:906323851366
Parent:
35:bce9ac485178
Child:
37:8021b3ce241a
Waits for errorTurn

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
sim642 35:bce9ac485178 27 MotorData m1Data = {PB_0, PC_0, PA_4}; //PWM, Dir1, Dir2
sim642 35:bce9ac485178 28 EncoderData enc1Data = {PC_1, PC_3, 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 36:906323851366 32 MotorData m2Data = {PB_7, PB_9, PA_6}; //PB7 = fault dir2 oli enne PC13
phairero 36:906323851366 33 EncoderData enc2Data = {PC_15, PC_14, 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
sim642 35:bce9ac485178 37 MotorData m3Data = {PA_15, PC_12, PC_10};
sim642 35:bce9ac485178 38 EncoderData enc3Data = {PC_11, 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
sim642 35:bce9ac485178 52 MotorData m6Data = {PB_3, PB_5, PA_7}; //PA_2 = TX; PA_3 (m6-fault) = RX DIR2 oli enne PA2
sim642 35:bce9ac485178 53 EncoderData enc6Data = {PA_10, PB_4, 102.083 * 64};
phairero 30:476ab400d765 54 EncoderMotor m6(m6Data, enc6Data, speedPIDData, turnPIDData, NULL);
phairero 32:9db688917ca3 55 DigitalIn s6(PB_10);
sim642 35:bce9ac485178 56
phairero 30:476ab400d765 57 const int MOTORS = 6;
phairero 30:476ab400d765 58 EncoderMotor* ms[MOTORS] = {&m1, &m2, &m3, &m4, &m5, &m6};
phairero 36:906323851366 59 volatile float mod = 0.25;
phairero 32:9db688917ca3 60 DigitalIn ss[MOTORS] = {s1, s2, s3, s4, s5, s6};
sim642 35:bce9ac485178 61
sim642 27:24a9ac72fe92 62 Ticker ticker;
sim642 27:24a9ac72fe92 63
sim642 2:cf0147952fb9 64 void rise()
sim642 2:cf0147952fb9 65 {
sim642 24:fb1827be6f7e 66 //pc.printf("rise\n");
sim642 28:fd21faad6dd8 67 //m1.drive(0);
phairero 36:906323851366 68 //mod = mod*(-1);
sim642 2:cf0147952fb9 69 }
phairero 0:0b7259fdb68a 70
sim642 2:cf0147952fb9 71 void fall()
sim642 2:cf0147952fb9 72 {
sim642 24:fb1827be6f7e 73 //pc.printf("fall\n");
sim642 28:fd21faad6dd8 74 //m1.drive(0.25);
sim642 2:cf0147952fb9 75 }
phairero 1:8b0322a353f4 76
sim642 27:24a9ac72fe92 77 void tick()
sim642 27:24a9ac72fe92 78 {
sim642 28:fd21faad6dd8 79 for (int i = 0; i < MOTORS; i++)
sim642 28:fd21faad6dd8 80 ms[i]->tick();
sim642 27:24a9ac72fe92 81 }
sim642 27:24a9ac72fe92 82
sim642 27:24a9ac72fe92 83 const float tickTime = 1.f / 60;
sim642 27:24a9ac72fe92 84
sim642 2:cf0147952fb9 85 int main()
sim642 3:2235787e78c4 86 {
phairero 31:8d8d31b6d8cd 87 pc.printf("MAIN\n");
sim642 3:2235787e78c4 88 bt.rise(&rise);
sim642 3:2235787e78c4 89 bt.fall(&fall);
sim642 27:24a9ac72fe92 90
sim642 28:fd21faad6dd8 91 //ms[active]->drive(0.25);
sim642 28:fd21faad6dd8 92
sim642 27:24a9ac72fe92 93 ticker.attach(&tick, tickTime);
sim642 5:7f800f61cb13 94
phairero 36:906323851366 95 //unsigned char volatile rfget;
phairero 33:ad2ebe22f8dd 96
phairero 36:906323851366 97 float volatile speed;
phairero 36:906323851366 98
phairero 36:906323851366 99 float volatile sum;
phairero 36:906323851366 100
sim642 25:a8bb69e99d6b 101
sim642 2:cf0147952fb9 102 while(1)
phairero 36:906323851366 103 {
phairero 36:906323851366 104
phairero 36:906323851366 105 speed = 0.925f;
phairero 36:906323851366 106 //int oldactive = active;
phairero 36:906323851366 107 //pc.scanf("%f", &speed);
phairero 36:906323851366 108
phairero 36:906323851366 109 if (speed != 0){
phairero 36:906323851366 110 speed = (speed*0.3f); //+ ((speed < 0) ? -1 : 1) * mod;
phairero 36:906323851366 111
phairero 36:906323851366 112 ms[0]->rotate(-0.75f,speed*3);
phairero 36:906323851366 113 ms[1]->rotate(-0.25f,speed);
phairero 36:906323851366 114 ms[2]->rotate(0.75f,speed*3);
phairero 36:906323851366 115 ms[3]->rotate(0.25f,speed);
phairero 36:906323851366 116 ms[4]->rotate(0.75f,speed*3);
phairero 36:906323851366 117 ms[5]->rotate(-0.25f,speed);
phairero 36:906323851366 118 tick();
phairero 36:906323851366 119 do {
phairero 36:906323851366 120 sum = 0;
phairero 36:906323851366 121 for (int i = 0; i < MOTORS; i++){
phairero 36:906323851366 122 pc.printf("%d,%f\n",i,ms[i]->errorTurn);
phairero 36:906323851366 123 sum+=abs(ms[i]->errorTurn);
phairero 36:906323851366 124 }
phairero 36:906323851366 125 pc.printf("1)%f\n",sum);
phairero 36:906323851366 126 } while( sum > 0.05f );
phairero 36:906323851366 127 ms[0]->rotate(-0.25f,speed);
phairero 36:906323851366 128 ms[1]->rotate(-0.75f,speed*3);
phairero 36:906323851366 129 ms[2]->rotate(0.25f,speed);
phairero 36:906323851366 130 ms[3]->rotate(0.75f,speed*3);
phairero 36:906323851366 131 ms[4]->rotate(0.25f,speed);
phairero 36:906323851366 132 ms[5]->rotate(-0.75f,speed*3);
phairero 36:906323851366 133 tick();
phairero 36:906323851366 134 do {
phairero 36:906323851366 135 sum = 0;
phairero 36:906323851366 136 for (int i = 0; i < MOTORS; i++){
phairero 36:906323851366 137 sum+=abs(ms[i]->errorTurn);
phairero 36:906323851366 138 }
phairero 36:906323851366 139 //pc.printf("2)%f\n",sum);
phairero 36:906323851366 140 } while( sum > 0.05f );
phairero 36:906323851366 141 wait(0.1f);
phairero 36:906323851366 142 }
phairero 36:906323851366 143 else {
phairero 36:906323851366 144 ms[0]->drive(0.0);
phairero 36:906323851366 145 ms[1]->drive(0.0);
phairero 36:906323851366 146 ms[2]->drive(0.0);
phairero 36:906323851366 147 ms[3]->drive(0.0);
phairero 36:906323851366 148 ms[4]->drive(0.0);
phairero 36:906323851366 149 ms[5]->drive(0.0);
phairero 36:906323851366 150 }
phairero 36:906323851366 151
phairero 36:906323851366 152 //pc.printf("%f", speed);
phairero 32:9db688917ca3 153 //info enkoodritelt
phairero 36:906323851366 154 /*
sim642 29:f905e66e1a6f 155 for (int i = 0; i < MOTORS; i++)
sim642 29:f905e66e1a6f 156 pc.printf("%ld ", ms[i]->encoder.getCount());
sim642 29:f905e66e1a6f 157 pc.printf("\n");
phairero 36:906323851366 158 */
phairero 33:ad2ebe22f8dd 159 /*
phairero 32:9db688917ca3 160 //info IRidelt
phairero 32:9db688917ca3 161 for (int i = 0; i < MOTORS; i++)
phairero 32:9db688917ca3 162 pc.printf("%d ", ss[i].read());
phairero 32:9db688917ca3 163 pc.printf("\n");
phairero 36:906323851366 164 wait(1.0f);
phairero 33:ad2ebe22f8dd 165 */
sim642 28:fd21faad6dd8 166 //pc.printf("%d", active);
phairero 31:8d8d31b6d8cd 167 //pc.scanf("%f", &turn);
sim642 23:d844cc906b66 168 //m.rotate(turn, 0.2);
phairero 31:8d8d31b6d8cd 169 //pc.scanf("%f %f", &rot, &speed);
sim642 28:fd21faad6dd8 170 //m1.rotate(rot, speed);
sim642 23:d844cc906b66 171 //m.drive(speed);
phairero 31:8d8d31b6d8cd 172 //pc.printf("%f %f\n", s.getTurnSpeed(), m.getSetSpeed());
phairero 31:8d8d31b6d8cd 173 //pc.printf("%f %f\n", m.s, m.getSetSpeed());
phairero 31:8d8d31b6d8cd 174 //pc.printf("%ld %f %f\n", m1.getEncoder().getCount(), m1.getEncoder().getTurn(), m1.getSetTurn());
phairero 31:8d8d31b6d8cd 175 //pc.printf("%f %f\n", m.getEncoder().getTurn(), m.getSetTurn());
phairero 36:906323851366 176
phairero 0:0b7259fdb68a 177 }
phairero 26:c865244ca3cf 178 }