Kiko Ishimoto / MotorClass

MotoｒDriveｒのクラス

Dependencies:  

Dependents:   MotorDriveDEBUG

MotorClass.cpp@0:842f92dcf668, 2015-08-18 (annotated)

Committer:
kikoaac
Date:
Tue Aug 18 01:29:19 2015 +0000
Revision:
0:842f92dcf668
MotorDrive????

;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kikoaac 0:842f92dcf668 1 #include "mbed.h"
kikoaac 0:842f92dcf668 2 #include "QEI.h"
kikoaac 0:842f92dcf668 3 #include "MotorClass.h"
kikoaac 0:842f92dcf668 4 MotorClass::MotorClass(I2C& _i2c ,QEI& _qei) : i2c(_i2c),wheel(_qei)
kikoaac 0:842f92dcf668 5 {
kikoaac 0:842f92dcf668 6 Action=0;
kikoaac 0:842f92dcf668 7 interval = 0.09;
kikoaac 0:842f92dcf668 8 AngOrSpe = -1;
kikoaac 0:842f92dcf668 9 //if(wheel.A == 1);
kikoaac 0:842f92dcf668 10 }
kikoaac 0:842f92dcf668 11 /*MotorClass::MotorClass(I2C& _i2c,char add) : i2c(_i2c)
kikoaac 0:842f92dcf668 12 {
kikoaac 0:842f92dcf668 13 addr = add;
kikoaac 0:842f92dcf668 14 }*/
kikoaac 0:842f92dcf668 15
kikoaac 0:842f92dcf668 16 bool MotorClass::SetRate(char rate)
kikoaac 0:842f92dcf668 17 {
kikoaac 0:842f92dcf668 18 return OutputOneData(PulsePerAngle,rate);
kikoaac 0:842f92dcf668 19 }
kikoaac 0:842f92dcf668 20 bool MotorClass::Setup(char action)
kikoaac 0:842f92dcf668 21 {
kikoaac 0:842f92dcf668 22
kikoaac 0:842f92dcf668 23 if(action==3||action==2)AngOrSpe=0;
kikoaac 0:842f92dcf668 24 else if(action==5||action==4)AngOrSpe=1;
kikoaac 0:842f92dcf668 25 else AngOrSpe=-1;
kikoaac 0:842f92dcf668 26 Action = action;
kikoaac 0:842f92dcf668 27 return OutputOneData(MotorMode, action );
kikoaac 0:842f92dcf668 28 }
kikoaac 0:842f92dcf668 29 bool MotorClass::Motor_PWM(float pwm)
kikoaac 0:842f92dcf668 30 {
kikoaac 0:842f92dcf668 31 PWM = pwm;
kikoaac 0:842f92dcf668 32 short pwm_ = pwm*0xffff;
kikoaac 0:842f92dcf668 33 char data[2] = {pwm_&0xff,pwm_>>8};
kikoaac 0:842f92dcf668 34 return OutputSomeData(MotorPWML,data,2);
kikoaac 0:842f92dcf668 35 }
kikoaac 0:842f92dcf668 36 bool MotorClass::Motor(char state,float pwm)
kikoaac 0:842f92dcf668 37 {
kikoaac 0:842f92dcf668 38 static short prev_state;
kikoaac 0:842f92dcf668 39 PWM = pwm;
kikoaac 0:842f92dcf668 40 short pwm_ = pwm*0xffff;
kikoaac 0:842f92dcf668 41 if(state == prev_state)
kikoaac 0:842f92dcf668 42 {
kikoaac 0:842f92dcf668 43 char data[2] = {pwm_&0xff,pwm_>>8};
kikoaac 0:842f92dcf668 44 return OutputSomeData(MotorPWML,data,2);
kikoaac 0:842f92dcf668 45 }
kikoaac 0:842f92dcf668 46 else
kikoaac 0:842f92dcf668 47 {
kikoaac 0:842f92dcf668 48 char data[3] = {state,pwm_&0xff,pwm_>>8};
kikoaac 0:842f92dcf668 49 prev_state=state;
kikoaac 0:842f92dcf668 50 return OutputSomeData(MotorState,data,3);
kikoaac 0:842f92dcf668 51 }
kikoaac 0:842f92dcf668 52 }
kikoaac 0:842f92dcf668 53 bool MotorClass::SetP(float p)
kikoaac 0:842f92dcf668 54 {
kikoaac 0:842f92dcf668 55 char P = char(p*10);
kikoaac 0:842f92dcf668 56 return OutputOneData(MotorP, P );
kikoaac 0:842f92dcf668 57 }
kikoaac 0:842f92dcf668 58 bool MotorClass::SetI(float i)
kikoaac 0:842f92dcf668 59 {
kikoaac 0:842f92dcf668 60 char I = char(i*10);
kikoaac 0:842f92dcf668 61 return OutputOneData(MotorI, I );
kikoaac 0:842f92dcf668 62 }
kikoaac 0:842f92dcf668 63 bool MotorClass::SetD(float d)
kikoaac 0:842f92dcf668 64 {
kikoaac 0:842f92dcf668 65 char D = char(d*10);
kikoaac 0:842f92dcf668 66 return OutputOneData(MotorD, D );
kikoaac 0:842f92dcf668 67 }
kikoaac 0:842f92dcf668 68 bool MotorClass::SetPID(float *pid)
kikoaac 0:842f92dcf668 69 {
kikoaac 0:842f92dcf668 70 char Pid[3] = {pid[0]*10,pid[1]*10,pid[2]*10};
kikoaac 0:842f92dcf668 71 return OutputSomeData(MotorP, Pid,3 );
kikoaac 0:842f92dcf668 72 }
kikoaac 0:842f92dcf668 73 bool MotorClass::SetTarget(short tar)
kikoaac 0:842f92dcf668 74 {
kikoaac 0:842f92dcf668 75 target=tar;
kikoaac 0:842f92dcf668 76 char data[2]={target&0xff,target>>8};
kikoaac 0:842f92dcf668 77 char reg[2] ={TargetAngleL ,TargetSpeedL };
kikoaac 0:842f92dcf668 78 return OutputSomeData(reg[AngOrSpe],data,2);
kikoaac 0:842f92dcf668 79 }
kikoaac 0:842f92dcf668 80 void MotorClass::StartQEIPut()
kikoaac 0:842f92dcf668 81 {
kikoaac 0:842f92dcf668 82 QeiInt.detach();
kikoaac 0:842f92dcf668 83 QeiInt.attach(this,&MotorClass::attach,interval);
kikoaac 0:842f92dcf668 84 }
kikoaac 0:842f92dcf668 85 void MotorClass::StopQEIPut()
kikoaac 0:842f92dcf668 86 {
kikoaac 0:842f92dcf668 87 AngOrSpe = -1;
kikoaac 0:842f92dcf668 88 QeiInt.detach();
kikoaac 0:842f92dcf668 89 }
kikoaac 0:842f92dcf668 90 void MotorClass::QEIAllSetup(short tar,float *pid)
kikoaac 0:842f92dcf668 91 {
kikoaac 0:842f92dcf668 92 SetPID(pid);
kikoaac 0:842f92dcf668 93 SetTarget(tar);
kikoaac 0:842f92dcf668 94 StartQEIPut();
kikoaac 0:842f92dcf668 95 SetRate(1);
kikoaac 0:842f92dcf668 96 }
kikoaac 0:842f92dcf668 97
kikoaac 0:842f92dcf668 98 char MotorClass::GetAddress()
kikoaac 0:842f92dcf668 99 {
kikoaac 0:842f92dcf668 100 return InputOneData(ADDRESS);
kikoaac 0:842f92dcf668 101 }
kikoaac 0:842f92dcf668 102 void MotorClass::UseAddress(char ad)
kikoaac 0:842f92dcf668 103 {
kikoaac 0:842f92dcf668 104 addr = ad;
kikoaac 0:842f92dcf668 105 }
kikoaac 0:842f92dcf668 106 char MotorClass::SetAddress(char ad)
kikoaac 0:842f92dcf668 107 {
kikoaac 0:842f92dcf668 108 char address = addr =ad;
kikoaac 0:842f92dcf668 109 char data=0;
kikoaac 0:842f92dcf668 110 /*do{
kikoaac 0:842f92dcf668 111 //data = InputOneData(0x3b);
kikoaac 0:842f92dcf668 112 wait(0.1);
kikoaac 0:842f92dcf668 113 }while(data==255);*/
kikoaac 0:842f92dcf668 114 char adD[2] = {data,address};
kikoaac 0:842f92dcf668 115 while(i2c.write(0x00,adD,2))wait(0.001);;
kikoaac 0:842f92dcf668 116
kikoaac 0:842f92dcf668 117 printf("%d,", data);
kikoaac 0:842f92dcf668 118 wait(0.5);
kikoaac 0:842f92dcf668 119 data = InputOneData(0x3a);
kikoaac 0:842f92dcf668 120 addr = address;
kikoaac 0:842f92dcf668 121 printf("%d\n", data);
kikoaac 0:842f92dcf668 122 wait(1.1);
kikoaac 0:842f92dcf668 123 return data;
kikoaac 0:842f92dcf668 124 }
kikoaac 0:842f92dcf668 125
kikoaac 0:842f92dcf668 126 void MotorClass::GetDebugData(float *data,int mode)
kikoaac 0:842f92dcf668 127 {
kikoaac 0:842f92dcf668 128 float rate[3]={1,1,1};
kikoaac 0:842f92dcf668 129 switch(mode)
kikoaac 0:842f92dcf668 130 {
kikoaac 0:842f92dcf668 131 case 0 :return;
kikoaac 0:842f92dcf668 132 case 1 :rate[1]=1000;break;
kikoaac 0:842f92dcf668 133 case 2 :rate[0]*=100,rate[1]*=100,rate[2]*=100;break;
kikoaac 0:842f92dcf668 134 case 3 :;
kikoaac 0:842f92dcf668 135 case 4 :rate[0]*=100,rate[1]*=100,rate[2]*=1000;break;
kikoaac 0:842f92dcf668 136 case 5 :rate[0]*=100,rate[1]*=100,rate[2]*=1000;break;
kikoaac 0:842f92dcf668 137 }
kikoaac 0:842f92dcf668 138 char input[6];
kikoaac 0:842f92dcf668 139 InputSomeData(0x70,input,6);
kikoaac 0:842f92dcf668 140 data[0]=short((input[0]<<8)|input[1])/rate[0];
kikoaac 0:842f92dcf668 141 data[1]=short((input[2]<<8)|input[3])/rate[1];
kikoaac 0:842f92dcf668 142 data[2]=short((input[4]<<8)|input[5])/rate[2];
kikoaac 0:842f92dcf668 143
kikoaac 0:842f92dcf668 144
kikoaac 0:842f92dcf668 145 }