File content as of revision 26:c865244ca3cf:

#include "mbed.h"
#include "EncoderMotor.hpp"
#include "SyncGroup.hpp"

InterruptIn bt(USER_BUTTON);
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};
SyncGroup sync;
    
// 1
MotorData m1Data = {PB_0, PC_0, PC_3}; //PWM, Dir1, Dir2
EncoderData enc1Data = {PC_1, PA_4, 102.083 * 64}; //EncA, encB  // https://www.pololu.com/product/2826
EncoderMotor m1(m1Data, enc1Data, speedPIDData, turnPIDData, NULL);

// 2
MotorData m2Data = {PB_7, PC_14, PC_13};  //PB7 = fault
EncoderData enc2Data = {PC_15, PH_0, 102.083 * 64};
EncoderMotor m2(m2Data, enc2Data, speedPIDData, turnPIDData, NULL);

// 3
MotorData m3Data = {PA_15, PC_11, PC_10}; 
EncoderData enc3Data = {PC_12, PA_13, 102.083 * 64};
EncoderMotor m3(m3Data, enc3Data, speedPIDData, turnPIDData, NULL);

// 4
MotorData m4Data = {PB_8, PC_6, PC_9}; 
EncoderData enc4Data = {PC_5, PA_12, 102.083 * 64};
EncoderMotor m4(m4Data, enc4Data, speedPIDData, turnPIDData, NULL);

// 5
MotorData m5Data = {PB_15, PB_1, PB_2}; 
EncoderData enc5Data = {PB_14, PB_13, 102.083 * 64};
EncoderMotor m5(m5Data, enc5Data, speedPIDData, turnPIDData, NULL);

// 6
//MotorData m6Data = {PB_3, PA_10, PA_2};  //PA_2 = TX; PA_3 (m6-fault) = RX
//EncoderData enc6Data = {PB_5, PB_4, 102.083 * 64};
//EncoderMotor m6(m6Data, enc6Data, speedPIDData, turnPIDData, NULL); 

//const int mitu = 5;

//EncoderMotor m[mitu] = {m1,m2,m3,m4,m5};//,m6};

//SpeedEncoder s(encData);

//PIDController ec(0.2, 0.1, 0.01);
// PIDController ec(0.3, 2.0, 0.02);
//PIDController ec(0.75, 2.0, 0.015);
//PIDController ec(0.8, 1.5, 0.017);
// PIDController ecRot(5.0, 0.1, 0.04);
//EncoderMotor m(mData, encData, ec, ecRot);
//Motor m(PB_0, PC_1, PC_0);

void rise()
{
    //pc.printf("rise\n");
    m1.drive(0);
}

void fall()
{
    //pc.printf("fall\n");
    m1.drive(0.25);
}

int main()
{   
    printf("MAIN\n");
    bt.rise(&rise);
    bt.fall(&fall);
    m1.setup();
    
    float rot, speed;
    //m.rotate(1.f, 0.5);
    
    //sync.update(NULL, 0.5);
    
    while(1)
    {
        //scanf("%f", &turn);
        //m.rotate(turn, 0.2);
        scanf("%f %f", &rot, &speed);
        m1.rotate(rot, speed);
        //m.drive(speed);
        //printf("%f %f\n", s.getTurnSpeed(), m.getSetSpeed());
        //printf("%f %f\n", m.s, m.getSetSpeed());
        printf("%ld %f %f\n", m1.getEncoder().getCount(), m1.getEncoder().getTurn(), m1.getSetTurn());
        //printf("%f %f\n", m.getEncoder().getTurn(), m.getSetTurn());
        wait(1.f / 60);
    }
}