MotorClass.cpp

00001 #include "mbed.h"
00002 #include "QEI.h"
00003 #include "MotorClass.h"
00004 MotorClass::MotorClass(I2C& _i2c ,QEI& _qei) : i2c(_i2c),wheel(_qei)
00005 {
00006     Action=0;
00007     interval = 0.09;
00008     AngOrSpe = -1;
00009     //if(wheel.A == 1);
00010 }
00011 /*MotorClass::MotorClass(I2C& _i2c,char add) : i2c(_i2c)
00012 {
00013     addr = add;
00014 }*/
00015 
00016 bool MotorClass::SetRate(char rate)
00017 {
00018     return OutputOneData(PulsePerAngle,rate);
00019 }
00020 bool MotorClass::Setup(char action)
00021 {
00022     
00023     if(action==3||action==2)AngOrSpe=0;
00024     else if(action==5||action==4)AngOrSpe=1;
00025     else AngOrSpe=-1;
00026     Action = action;
00027     return OutputOneData(MotorMode, action );
00028 }
00029 bool MotorClass::Motor_PWM(float pwm)
00030 {
00031     PWM = pwm;
00032     short pwm_ = pwm*0xffff;    
00033     char data[2] = {pwm_&0xff,pwm_>>8};
00034     return OutputSomeData(MotorPWML,data,2);
00035 }
00036 bool MotorClass::Motor(char state,float pwm)
00037 {
00038     static short prev_state;
00039     PWM = pwm;
00040     short pwm_ = pwm*0xffff;
00041     if(state == prev_state)
00042     {
00043         char data[2] = {pwm_&0xff,pwm_>>8};        
00044         return OutputSomeData(MotorPWML,data,2);
00045     }
00046     else 
00047     {
00048         char data[3] = {state,pwm_&0xff,pwm_>>8};
00049         prev_state=state;
00050         return OutputSomeData(MotorState,data,3);
00051     }
00052 }
00053 bool MotorClass::SetP(float p)
00054 {
00055     char P = char(p*10);
00056     return OutputOneData(MotorP, P );
00057 }
00058 bool MotorClass::SetI(float i)
00059 {
00060     char I = char(i*10);
00061     return OutputOneData(MotorI, I );
00062 }
00063 bool MotorClass::SetD(float d)
00064 {
00065     char D = char(d*10);    
00066     return OutputOneData(MotorD, D );
00067 }
00068 bool MotorClass::SetPID(float *pid)
00069 {
00070     char Pid[3] = {pid[0]*10,pid[1]*10,pid[2]*10};
00071     return OutputSomeData(MotorP, Pid,3 );
00072 }
00073 bool MotorClass::SetTarget(short tar)
00074 {
00075     target=tar;
00076     char data[2]={target&0xff,target>>8};
00077     char reg[2] ={TargetAngleL ,TargetSpeedL };
00078     return OutputSomeData(reg[AngOrSpe],data,2);
00079 }
00080 void MotorClass::StartQEIPut()
00081 {
00082     QeiInt.detach();
00083     QeiInt.attach(this,&MotorClass::attach,interval);
00084 }
00085 void MotorClass::StopQEIPut()
00086 {
00087     AngOrSpe = -1;
00088     QeiInt.detach();
00089 }
00090 void  MotorClass::QEIAllSetup(short tar,float *pid)
00091 {
00092     SetPID(pid);
00093     SetTarget(tar);
00094     StartQEIPut();
00095     SetRate(1);
00096 }
00097 
00098 char MotorClass::GetAddress()
00099 {
00100      return InputOneData(ADDRESS);
00101 }
00102 void MotorClass::UseAddress(char ad)
00103 {
00104     addr = ad;
00105 }
00106 char MotorClass::SetAddress(char ad)
00107 {
00108     char address = addr =ad;
00109     char data=0;
00110     /*do{
00111     //data = InputOneData(0x3b);
00112     wait(0.1);
00113     }while(data==255);*/
00114     char adD[2] = {data,address};
00115     while(i2c.write(0x00,adD,2))wait(0.001);;
00116     
00117         printf("%d,", data);
00118         wait(0.5);
00119     data = InputOneData(0x3a);
00120     addr = address;
00121         printf("%d\n", data);
00122         wait(1.1);
00123     return data;
00124 }
00125 
00126 void MotorClass::GetDebugData(float *data,int mode)
00127 {
00128     float rate[3]={1,1,1};
00129     switch(mode)
00130     {
00131         case 0 :return;
00132         case 1 :rate[1]=1000;break;
00133         case 2 :rate[0]*=100,rate[1]*=100,rate[2]*=100;break;
00134         case 3 :;
00135         case 4 :rate[0]*=100,rate[1]*=100,rate[2]*=1000;break;
00136         case 5 :rate[0]*=100,rate[1]*=100,rate[2]*=1000;break;
00137     }
00138     char input[6];
00139     InputSomeData(0x70,input,6);
00140     data[0]=short((input[0]<<8)|input[1])/rate[0];
00141     data[1]=short((input[2]<<8)|input[3])/rate[1];
00142     data[2]=short((input[4]<<8)|input[5])/rate[2];
00143     
00144     
00145 }