File content as of revision 0:842f92dcf668:

#include "mbed.h"
#include "QEI.h"
#include "MotorClass.h"
MotorClass::MotorClass(I2C& _i2c ,QEI& _qei) : i2c(_i2c),wheel(_qei)
{
    Action=0;
    interval = 0.09;
    AngOrSpe = -1;
    //if(wheel.A == 1);
}
/*MotorClass::MotorClass(I2C& _i2c,char add) : i2c(_i2c)
{
    addr = add;
}*/

bool MotorClass::SetRate(char rate)
{
    return OutputOneData(PulsePerAngle,rate);
}
bool MotorClass::Setup(char action)
{
    
    if(action==3||action==2)AngOrSpe=0;
    else if(action==5||action==4)AngOrSpe=1;
    else AngOrSpe=-1;
    Action = action;
    return OutputOneData(MotorMode, action );
}
bool MotorClass::Motor_PWM(float pwm)
{
    PWM = pwm;
    short pwm_ = pwm*0xffff;    
    char data[2] = {pwm_&0xff,pwm_>>8};
    return OutputSomeData(MotorPWML,data,2);
}
bool MotorClass::Motor(char state,float pwm)
{
    static short prev_state;
    PWM = pwm;
    short pwm_ = pwm*0xffff;
    if(state == prev_state)
    {
        char data[2] = {pwm_&0xff,pwm_>>8};        
        return OutputSomeData(MotorPWML,data,2);
    }
    else 
    {
        char data[3] = {state,pwm_&0xff,pwm_>>8};
        prev_state=state;
        return OutputSomeData(MotorState,data,3);
    }
}
bool MotorClass::SetP(float p)
{
    char P = char(p*10);
    return OutputOneData(MotorP, P );
}
bool MotorClass::SetI(float i)
{
    char I = char(i*10);
    return OutputOneData(MotorI, I );
}
bool MotorClass::SetD(float d)
{
    char D = char(d*10);    
    return OutputOneData(MotorD, D );
}
bool MotorClass::SetPID(float *pid)
{
    char Pid[3] = {pid[0]*10,pid[1]*10,pid[2]*10};
    return OutputSomeData(MotorP, Pid,3 );
}
bool MotorClass::SetTarget(short tar)
{
    target=tar;
    char data[2]={target&0xff,target>>8};
    char reg[2] ={TargetAngleL ,TargetSpeedL };
    return OutputSomeData(reg[AngOrSpe],data,2);
}
void MotorClass::StartQEIPut()
{
    QeiInt.detach();
    QeiInt.attach(this,&MotorClass::attach,interval);
}
void MotorClass::StopQEIPut()
{
    AngOrSpe = -1;
    QeiInt.detach();
}
void  MotorClass::QEIAllSetup(short tar,float *pid)
{
    SetPID(pid);
    SetTarget(tar);
    StartQEIPut();
    SetRate(1);
}

char MotorClass::GetAddress()
{
     return InputOneData(ADDRESS);
}
void MotorClass::UseAddress(char ad)
{
    addr = ad;
}
char MotorClass::SetAddress(char ad)
{
    char address = addr =ad;
    char data=0;
    /*do{
    //data = InputOneData(0x3b);
    wait(0.1);
    }while(data==255);*/
    char adD[2] = {data,address};
    while(i2c.write(0x00,adD,2))wait(0.001);;
    
        printf("%d,", data);
        wait(0.5);
    data = InputOneData(0x3a);
    addr = address;
        printf("%d\n", data);
        wait(1.1);
    return data;
}

void MotorClass::GetDebugData(float *data,int mode)
{
    float rate[3]={1,1,1};
    switch(mode)
    {
        case 0 :return;
        case 1 :rate[1]=1000;break;
        case 2 :rate[0]*=100,rate[1]*=100,rate[2]*=100;break;
        case 3 :;
        case 4 :rate[0]*=100,rate[1]*=100,rate[2]*=1000;break;
        case 5 :rate[0]*=100,rate[1]*=100,rate[2]*=1000;break;
    }
    char input[6];
    InputSomeData(0x70,input,6);
    data[0]=short((input[0]<<8)|input[1])/rate[0];
    data[1]=short((input[2]<<8)|input[3])/rate[1];
    data[2]=short((input[4]<<8)|input[5])/rate[2];
    
    
}