2016/05/06 交ロボの移植プログラム 移動only
Dependencies: mbed
main.cpp
- Committer:
- eil4nyqn
- Date:
- 2016-05-06
- Revision:
- 2:3bf43044ef9f
- Parent:
- 1:4f5a22371fff
File content as of revision 2:3bf43044ef9f:
#include "mbed.h" #define ratio 0.3 #define conratio (1.0-ratio) #define PI 3.141592 #define qPI 0.785398 DigitalOut leds[8]={PA_11,PB_12,PB_2,PB_1,PB_15,PB_14,PB_13,PC_4}; I2C i2c(I2C_SDA,I2C_SCL); Serial FEP02(PC_6,PA_12); Serial pc(USBTX,USBRX); char data[5]={0}; bool recFrag=0; const int addr[4]={0x10,0x23,0x56,0x76}; void ledsReset(){ int i=0; while(i<8){ leds[i]=0; i++; } } //今は使ってない受信関数 void getConData(){ int i=0; if(FEP02.getc()==255){ recFrag=1; while(i<5){ data[i]=FEP02.getc(); i++; } } //pc.printf("Recieved\r\n"); } //モーターの出力を計算する関数 void MotorOut(char MDFL[],char MDFR[],char MDRL[],char MDRR[]){ int dig; double rad,turn; double prePwmDuty[4],PwmDuty[4]; int i=0,n=0; //受信データから元の角度に変換 dig = data[0]+data[1]*128; //radianに変換 rad = (dig/180.0)*PI; //角度が360°の時モーター出力0 if(dig==360){ while(n<4){ prePwmDuty[n]=0; n++; } }else{ //各モータへの出力を算出 prePwmDuty[0]=1.41421356*sin(rad+qPI);//*conratio; prePwmDuty[1]=1.41421356*sin(rad-qPI);//*conratio; prePwmDuty[2]=prePwmDuty[1]; prePwmDuty[3]=prePwmDuty[0]; } //車体の回転のスピードを算出 turn = (data[2]-63)/64.0; //turn = 0; //各モーターに回転量を上乗せ prePwmDuty[0]+=turn;//*conratio;//*ratio; prePwmDuty[1]-=turn;//*conratio;//*ratio; prePwmDuty[2]+=turn;//*conratio;//*ratio; prePwmDuty[3]-=turn;//*conratio;//*ratio; while(i<4){ //値を-1.0~1.0の範囲だからその範囲に収める if(prePwmDuty[i]<-1.0) prePwmDuty[i]=-1.0; else if(prePwmDuty[i]>1.0) prePwmDuty[i]=1.0; //値を0~1.0の範囲に変換 PwmDuty[i]=(fabs(prePwmDuty[i]+1.0))/2; i++; } //送信できるデータは1byteだからその範囲に変換 MDFL[0] = PwmDuty[0]*255; MDFR[0] = PwmDuty[1]*255; MDRL[0] = PwmDuty[2]*255; MDRR[0] = PwmDuty[3]*255; //printf("%f,%f,%f,%f\r\n",PwmDuty[0],PwmDuty[1],PwmDuty[2],PwmDuty[3]); } void MotorReset(){ char data[2]={127,0}; int i=0; while(i<4){ i2c.write(addr[i],data,2,false); i++; } } void MotorRun(){ char data[2]={255,0}; int i=0; while(i<4){ i2c.write(addr[i],data,2,false); i++; } } int main() { char MDFL[2],MDFR[2],MDRL[2],MDRR[2]; int i2cVAL=0; int i=0; //I2Cの通信速度設定 i2c.frequency(300000); ledsReset(); //シリアルのボーレートを19200に設定 FEP02.baud(19200); /* MotorRun(); wait(3); */ //モータの出力をリセット MotorReset(); //FEP02.attach(&getConData); leds[0]=1; while(1) { i2cVAL=0; i=0; //FEP-02の受信シーケンス if(FEP02.getc()==255){ while(i<5){ data[i]=FEP02.getc(); i++; } //モータへの出力を算出する関数へ飛ぶ MotorOut(MDFL,MDFR,MDRL,MDRR); //各ドライバへI2Cラインを用いてデータ送信 i2cVAL += i2c.write(addr[0],MDFL,2,false); i2cVAL += i2c.write(addr[1],MDFR,2,false); i2cVAL += i2c.write(addr[2],MDRL,2,false); i2cVAL += i2c.write(addr[3],MDRR,2,false); printf("%d\r\n",i2cVAL); printf("%d,%d,%d,%d\r\n",MDFL[0],MDFR[0],MDRL[0],MDRR[0]); leds[0]=!leds[0]; leds[1]=1; //LEDが光ったらI2Cラインでミスが発生 if(i2cVAL) leds[2]=1; else leds[2]=0; } leds[1]=0; } }