File content as of revision 47:4f418a4b0051:

#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 rxSpeed;
int volatile rxDir;
int volatile rxTurn;

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++)
    {
        ms[i]->drive(0.60f);
    }
    
    bool done;
    do
    {
        done = true;
        for (int i = 0; i < MOTORS; i++)
        {
            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.0625f, 0.75f);
    ms[1]->rotate(0.5f - 0.0625f, 0.75f);
    ms[2]->rotate(0.5f + 0.0625f, 0.75f);
    ms[3]->rotate(0.5f - 0.0625f, 0.75f);
    ms[4]->rotate(0.5f + 0.0625f, 0.75f);
    ms[5]->rotate(0.5f - 0.0625f, 0.75f);
    waitAllRotate();
}

float groundDivide = 4;

void moveOne()
{
    
    ms[0]->rotate(dir*0.875f,speed);
    ms[1]->rotate(dir*0.125f,speed/groundDivide);
    ms[2]->rotate(dir*0.875f,speed);
    ms[3]->rotate(dir*0.125f,speed/groundDivide);
    ms[4]->rotate(dir*0.875f,speed);
    ms[5]->rotate(dir*0.125f,speed/groundDivide);
    waitAllRotate();
}

void moveTwo(){
    ms[0]->rotate(dir*0.125f,speed/groundDivide);
    ms[1]->rotate(dir*0.875f,speed);
    ms[2]->rotate(dir*0.125f,speed/groundDivide);
    ms[3]->rotate(dir*0.875f,speed);
    ms[4]->rotate(dir*0.125f,speed/groundDivide);
    ms[5]->rotate(dir*0.875f,speed);
    waitAllRotate();
}

void turnOne(){
    ms[0]->rotate(turn*dir*0.875f,speed);
    ms[1]->rotate(turn*dir*0.125f,speed/groundDivide);
    ms[2]->rotate(-turn*dir*0.125f,speed/groundDivide);
    ms[3]->rotate(-turn*dir*0.875f,speed);
    ms[4]->rotate(-turn*dir*0.125f,speed/groundDivide);
    ms[5]->rotate(turn*dir*0.125f,speed/groundDivide);
    waitAllRotate();
}

void turnTwo(){
    ms[0]->rotate(turn*dir*0.125f,speed/groundDivide);
    ms[1]->rotate(0.0f, 1.0f);
    ms[2]->rotate(-turn*dir*0.875f,speed);
    ms[3]->rotate(0.0f, 1.0f);
    ms[4]->rotate(-turn*dir*0.875f,speed);
    ms[5]->rotate(0.0f, 1.0f);
    waitAllRotate();
    }
    
void turnThree(){    
    ms[0]->rotate(0.0f, 1.0f);
    ms[1]->rotate(turn*dir*0.875f,speed);
    ms[2]->rotate(0.0f, 1.0f);
    ms[3]->rotate(-turn*dir*0.125f,speed/groundDivide);
    ms[4]->rotate(0.0f, 1.0f);
    ms[5]->rotate(turn*dir*0.875f,speed);
    waitAllRotate();
}

void rfRx()
{
    int b = pc.getc();
    int rfSpeed = b & 0x3F;
    
    rxSpeed = rfSpeed / 10.f * 2;
    
    switch ((b >> 6) & 0x3)
    {
        case 0: // fwd
            rxDir = 1;
            rxTurn = 0;
            break;
        
        case 1: // bwd
            rxDir = -1;
            rxTurn = 0;
            break;
        
        case 2: // left
            rxDir = 1;
            rxTurn = -1;
            break;
        
        case 3: // right
            rxDir = 1;
            rxTurn = 1;
            break;
    }
}

int main()
{
    pc.attach(&rfRx);
    
    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;
    
    rxSpeed = 0.0f;
    rxDir = 1;
    rxTurn = 0;
    
    pc.printf("start\n");
    while(1)
    {
        speed = rxSpeed;
        dir = rxDir;
        turn = rxTurn;
        
        //pc.printf("%.2f %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 (turn == 0) {
                if (dir == 1){
                    moveOne();
                    moveTwo();
                }
                else {
                    moveTwo();
                    moveOne();
                }
            }
            else {
                if ((turn*dir) == 1){
                    turnOne();
                    turnTwo();
                    turnThree();
                }
                else {
                    turnThree();
                    turnTwo();
                    turnOne();
                }    
            }
        }
        else {
            ms[0]->rotate(0.0f, 1.0f);
            ms[1]->rotate(0.0f, 1.0f);
            ms[2]->rotate(0.0f, 1.0f);
            ms[3]->rotate(0.0f, 1.0f);
            ms[4]->rotate(0.0f, 1.0f);
            ms[5]->rotate(0.0f, 1.0f);
        }
        
        //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());
        
    }
}