Rauno U
/
Miisu
Six crescent shaped legs
main.cpp
- Committer:
- phairero
- Date:
- 2016-06-16
- Revision:
- 43:0627a2245b9d
- Parent:
- 42:7fa713d5d1af
- Child:
- 44:c2acf8d5e191
File content as of revision 43:0627a2245b9d:
#include "mbed.h" #include "EncoderMotor.hpp" InterruptIn bt(USER_BUTTON); Serial pc(PA_0, PA_1); //RF //Serial pc(SERIAL_TX, SERIAL_RX); //Serial pc(USBTX, USBRX); //PIDData speedPIDData = {0.3f, 2.0f, 0.02f}; //PIDData turnPIDData = {5.0f, 0.1f, 0.04f}; PIDData speedPIDData = {0.5f, 0.0f, 0.0f}; PIDData turnPIDData = {30.0f, 0.01f, 1.0f}; /* PWM timer channel M1 1 2N M2 4 2 M3 2 1 M4 4 4 M5 1 3N M6 2 2 */ // 1 MotorData m1Data = {PB_0, PA_4, PC_0}; //PWM, Dir1, Dir2 EncoderData enc1Data = {PC_3, PC_1, 102.083 * 64}; //EncA, encB // https://www.pololu.com/product/2826 EncoderMotor m1(m1Data, enc1Data, speedPIDData, turnPIDData); DigitalIn s1(PA_8); // 2 MotorData m2Data = {PB_7, PA_6, PB_9}; //PB7 = fault dir2 oli enne PC13 EncoderData enc2Data = {PC_14, PC_15, 102.083 * 64}; EncoderMotor m2(m2Data, enc2Data, speedPIDData, turnPIDData); DigitalIn s2(PH_1); // 3 MotorData m3Data = {PA_15, PC_12, PC_10}; EncoderData enc3Data = {PC_11, PA_9, 102.083 * 64}; //B oli varem PA_13 EncoderMotor m3(m3Data, enc3Data, speedPIDData, turnPIDData); DigitalIn s3(PA_14); // 4 MotorData m4Data = {PB_8, PC_6, PC_9}; EncoderData enc4Data = {PC_5, PA_12, 102.083 * 64}; EncoderMotor m4(m4Data, enc4Data, speedPIDData, turnPIDData); DigitalIn s4(PA_11); // 5 MotorData m5Data = {PB_15, PB_1, PB_2}; EncoderData enc5Data = {PC_7, PB_6, 102.083 * 64}; //B oli varem PB_13, A oli varem PB_14 EncoderMotor m5(m5Data, enc5Data, speedPIDData, turnPIDData); DigitalIn s5(PC_4); // 6 MotorData m6Data = {PB_3, PA_7, PB_5}; //PA_2 = TX; PA_3 (m6-fault) = RX DIR2 oli enne PA2 EncoderData enc6Data = {PB_4, PA_10, 102.083 * 64}; EncoderMotor m6(m6Data, enc6Data, speedPIDData, turnPIDData); DigitalIn s6(PB_10); const int MOTORS = 6; EncoderMotor* ms[MOTORS] = {&m1, &m2, &m3, &m4, &m5, &m6}; DigitalIn ss[MOTORS] = {s1, s2, s3, s4, s5, s6}; float volatile speed; int volatile dir; int volatile turn; Ticker ticker; void rise() { //pc.printf("rise\n"); //m1.drive(0); //mod = mod*(-1); } void fall() { //pc.printf("fall\n"); //m1.drive(0.25); } void tick() { for (int i = 0; i < MOTORS; i++) ms[i]->tick(); } const float tickTime = 1.f / 60; void waitAllRotate() { tick(); float sum; do { sum = 0; for (int i = 0; i < MOTORS; i++) sum += abs(ms[i]->errorTurn); pc.printf(""); //pc.printf("%f\n",sum); } while(sum > 0.05f); wait(0.1f); } void calibrateLegs() { for (int i = 0; i < MOTORS; i++) { if (i != 3) ms[i]->drive(0.2f); } bool done; do { done = true; for (int i = 0; i < MOTORS; i++) { if (i != 3) { if (ss[i].read()) done = false; else ms[i]->drive(0.f); } } } while (!done); for (int i = 0; i < MOTORS; i++) ms[i]->getEncoder().reset(); pc.printf("done\n"); } void standUp() { ms[0]->rotate(0.5f + 0.125f, 0.75f); ms[1]->rotate(0.5f - 0.125f, 0.75f); ms[2]->rotate(0.5f + 0.125f, 0.75f); ms[3]->rotate(0.5f - 0.125f, 0.75f); ms[4]->rotate(0.5f + 0.125f, 0.75f); ms[5]->rotate(0.5f - 0.125f, 0.75f); waitAllRotate(); } void moveOne() { ms[0]->rotate(dir*0.75f,speed); ms[1]->rotate(dir*0.25f,speed/3); ms[2]->rotate(dir*0.75f,speed); ms[3]->rotate(dir*0.25f,speed/3); ms[4]->rotate(dir*0.75f,speed); ms[5]->rotate(dir*0.25f,speed/3); waitAllRotate(); } void moveTwo(){ ms[0]->rotate(dir*0.25f,speed/3); ms[1]->rotate(dir*0.75f,speed); ms[2]->rotate(dir*0.25f,speed/3); ms[3]->rotate(dir*0.75f,speed); ms[4]->rotate(dir*0.25f,speed/3); ms[5]->rotate(dir*0.75f,speed); waitAllRotate(); } int main() { pc.printf("MAIN\n"); bt.rise(&rise); bt.fall(&fall); //ms[active]->drive(0.25); ticker.attach(&tick, tickTime); //unsigned char volatile rfget; calibrateLegs(); standUp(); //float volatile sum; speed = 0.0f; dir = 1; turn = 0; pc.printf("start\n"); while(1) { //speed = 1.5f; //speed = 0; //int oldactive = active; if (pc.readable()){ pc.scanf("%f %d %d", &speed, &dir, &turn); if (speed >= 10){ speed = speed - (((int)(speed/10))*10); } } pc.printf("%f %d%d\n",speed,dir,turn); if (speed != 0){ //for (int i = 0; i < MOTORS; i++) // pc.printf("%ld ", ms[i]->encoder.getCount()); //pc.printf("\n"); //speed = (rfget*0.3f); //+ ((speed < 0) ? -1 : 1) * mod; if (dir == 1){ moveOne(); moveTwo(); } else { moveTwo(); moveOne(); } /*ms[0]->rotate(dir*0.75f,speed); ms[1]->rotate(dir*0.25f,speed/3); ms[2]->rotate(dir*0.75f,speed); ms[3]->rotate(dir*0.25f,speed/3); ms[4]->rotate(dir*0.75f,speed); ms[5]->rotate(dir*0.25f,speed/3); waitAllRotate(); ms[0]->rotate(dir*0.25f,speed/3); ms[1]->rotate(dir*0.75f,speed); ms[2]->rotate(dir*0.25f,speed/3); ms[3]->rotate(dir*0.75f,speed); ms[4]->rotate(dir*0.25f,speed/3); ms[5]->rotate(dir*0.75f,speed); waitAllRotate();*/ } else { ms[0]->drive(0.0); ms[1]->drive(0.0); ms[2]->drive(0.0); ms[3]->drive(0.0); ms[4]->drive(0.0); ms[5]->drive(0.0); } //pc.printf("%f", speed); //info enkoodritelt /* for (int i = 0; i < MOTORS; i++) pc.printf("%ld ", ms[i]->encoder.getCount()); pc.printf("\n"); */ //info IRidelt /*for (int i = 0; i < MOTORS; i++) pc.printf("%d ", ss[i].read()); pc.printf("\n"); wait(1.0f);*/ //pc.printf("%d", active); //pc.scanf("%f", &turn); //m.rotate(turn, 0.2); //pc.scanf("%f %f", &rot, &speed); //m1.rotate(rot, speed); //m.drive(speed); //pc.printf("%f %f\n", s.getTurnSpeed(), m.getSetSpeed()); //pc.printf("%f %f\n", m.s, m.getSetSpeed()); //pc.printf("%ld %f %f\n", m1.getEncoder().getCount(), m1.getEncoder().getTurn(), m1.getSetTurn()); //pc.printf("%f %f\n", m.getEncoder().getTurn(), m.getSetTurn()); } }