NagaokaRoboticsClub_mbedTeam / Mbed 2 deprecated MecanumWheel3

2016/05/06 交ロボの移植プログラム　移動only

Dependencies:   mbed

main.cpp@2:3bf43044ef9f, 2016-05-06 (annotated)

Committer:
eil4nyqn
Date:
Fri May 06 09:13:54 2016 +0000
Revision:
2:3bf43044ef9f
Parent:
1:4f5a22371fff 
comment????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
eil4nyqn 0:165e50fc913f 1 #include "mbed.h"
eil4nyqn 0:165e50fc913f 2
eil4nyqn 0:165e50fc913f 3 #define ratio 0.3
eil4nyqn 0:165e50fc913f 4 #define conratio (1.0-ratio)
eil4nyqn 0:165e50fc913f 5
eil4nyqn 0:165e50fc913f 6 #define PI 3.141592
eil4nyqn 0:165e50fc913f 7 #define qPI 0.785398
eil4nyqn 0:165e50fc913f 8
eil4nyqn 0:165e50fc913f 9 DigitalOut leds[8]={PA_11,PB_12,PB_2,PB_1,PB_15,PB_14,PB_13,PC_4};
eil4nyqn 0:165e50fc913f 10 I2C i2c(I2C_SDA,I2C_SCL);
eil4nyqn 0:165e50fc913f 11 Serial FEP02(PC_6,PA_12);
eil4nyqn 0:165e50fc913f 12 Serial pc(USBTX,USBRX);
eil4nyqn 0:165e50fc913f 13
eil4nyqn 0:165e50fc913f 14 char data[5]={0};
eil4nyqn 0:165e50fc913f 15 bool recFrag=0;
eil4nyqn 0:165e50fc913f 16 const int addr[4]={0x10,0x23,0x56,0x76};
eil4nyqn 0:165e50fc913f 17
eil4nyqn 0:165e50fc913f 18 void ledsReset(){
eil4nyqn 0:165e50fc913f 19 int i=0;
eil4nyqn 0:165e50fc913f 20 while(i<8){
eil4nyqn 0:165e50fc913f 21 leds[i]=0;
eil4nyqn 0:165e50fc913f 22 i++;
eil4nyqn 0:165e50fc913f 23 }
eil4nyqn 0:165e50fc913f 24 }
eil4nyqn 2:3bf43044ef9f 25 //今は使ってない受信関数
eil4nyqn 0:165e50fc913f 26 void getConData(){
eil4nyqn 0:165e50fc913f 27 int i=0;
eil4nyqn 0:165e50fc913f 28 if(FEP02.getc()==255){
eil4nyqn 0:165e50fc913f 29 recFrag=1;
eil4nyqn 0:165e50fc913f 30 while(i<5){
eil4nyqn 0:165e50fc913f 31 data[i]=FEP02.getc();
eil4nyqn 0:165e50fc913f 32 i++;
eil4nyqn 0:165e50fc913f 33 }
eil4nyqn 0:165e50fc913f 34 }
eil4nyqn 0:165e50fc913f 35 //pc.printf("Recieved\r\n");
eil4nyqn 0:165e50fc913f 36 }
eil4nyqn 2:3bf43044ef9f 37 //モーターの出力を計算する関数
eil4nyqn 0:165e50fc913f 38 void MotorOut(char MDFL[],char MDFR[],char MDRL[],char MDRR[]){
eil4nyqn 0:165e50fc913f 39 int dig;
eil4nyqn 0:165e50fc913f 40 double rad,turn;
eil4nyqn 0:165e50fc913f 41 double prePwmDuty[4],PwmDuty[4];
eil4nyqn 0:165e50fc913f 42 int i=0,n=0;
eil4nyqn 2:3bf43044ef9f 43 //受信データから元の角度に変換
eil4nyqn 0:165e50fc913f 44 dig = data[0]+data[1]*128;
eil4nyqn 2:3bf43044ef9f 45 //radianに変換
eil4nyqn 0:165e50fc913f 46 rad = (dig/180.0)*PI;
eil4nyqn 2:3bf43044ef9f 47 //角度が360°の時モーター出力0
eil4nyqn 0:165e50fc913f 48 if(dig==360){
eil4nyqn 0:165e50fc913f 49 while(n<4){
eil4nyqn 0:165e50fc913f 50 prePwmDuty[n]=0;
eil4nyqn 0:165e50fc913f 51 n++;
eil4nyqn 0:165e50fc913f 52 }
eil4nyqn 0:165e50fc913f 53 }else{
eil4nyqn 2:3bf43044ef9f 54 //各モータへの出力を算出
eil4nyqn 1:4f5a22371fff 55 prePwmDuty[0]=1.41421356*sin(rad+qPI);//*conratio;
eil4nyqn 1:4f5a22371fff 56 prePwmDuty[1]=1.41421356*sin(rad-qPI);//*conratio;
eil4nyqn 0:165e50fc913f 57 prePwmDuty[2]=prePwmDuty[1];
eil4nyqn 0:165e50fc913f 58 prePwmDuty[3]=prePwmDuty[0];
eil4nyqn 0:165e50fc913f 59 }
eil4nyqn 2:3bf43044ef9f 60 //車体の回転のスピードを算出
eil4nyqn 0:165e50fc913f 61 turn = (data[2]-63)/64.0;
eil4nyqn 1:4f5a22371fff 62 //turn = 0;
eil4nyqn 2:3bf43044ef9f 63 //各モーターに回転量を上乗せ
eil4nyqn 1:4f5a22371fff 64 prePwmDuty[0]+=turn;//*conratio;//*ratio;
eil4nyqn 1:4f5a22371fff 65 prePwmDuty[1]-=turn;//*conratio;//*ratio;
eil4nyqn 1:4f5a22371fff 66 prePwmDuty[2]+=turn;//*conratio;//*ratio;
eil4nyqn 1:4f5a22371fff 67 prePwmDuty[3]-=turn;//*conratio;//*ratio;
eil4nyqn 0:165e50fc913f 68
eil4nyqn 0:165e50fc913f 69 while(i<4){
eil4nyqn 2:3bf43044ef9f 70 //値を-1.0~1.0の範囲だからその範囲に収める
eil4nyqn 0:165e50fc913f 71 if(prePwmDuty[i]<-1.0) prePwmDuty[i]=-1.0;
eil4nyqn 0:165e50fc913f 72 else if(prePwmDuty[i]>1.0) prePwmDuty[i]=1.0;
eil4nyqn 2:3bf43044ef9f 73 //値を0~1.0の範囲に変換
eil4nyqn 0:165e50fc913f 74 PwmDuty[i]=(fabs(prePwmDuty[i]+1.0))/2;
eil4nyqn 0:165e50fc913f 75 i++;
eil4nyqn 0:165e50fc913f 76 }
eil4nyqn 2:3bf43044ef9f 77 //送信できるデータは1byteだからその範囲に変換
eil4nyqn 0:165e50fc913f 78 MDFL[0] = PwmDuty[0]*255;
eil4nyqn 0:165e50fc913f 79 MDFR[0] = PwmDuty[1]*255;
eil4nyqn 0:165e50fc913f 80 MDRL[0] = PwmDuty[2]*255;
eil4nyqn 0:165e50fc913f 81 MDRR[0] = PwmDuty[3]*255;
eil4nyqn 0:165e50fc913f 82
eil4nyqn 0:165e50fc913f 83 //printf("%f,%f,%f,%f\r\n",PwmDuty[0],PwmDuty[1],PwmDuty[2],PwmDuty[3]);
eil4nyqn 0:165e50fc913f 84 }
eil4nyqn 0:165e50fc913f 85
eil4nyqn 0:165e50fc913f 86 void MotorReset(){
eil4nyqn 0:165e50fc913f 87 char data[2]={127,0};
eil4nyqn 0:165e50fc913f 88 int i=0;
eil4nyqn 0:165e50fc913f 89 while(i<4){
eil4nyqn 0:165e50fc913f 90 i2c.write(addr[i],data,2,false);
eil4nyqn 0:165e50fc913f 91 i++;
eil4nyqn 0:165e50fc913f 92 }
eil4nyqn 0:165e50fc913f 93 }
eil4nyqn 0:165e50fc913f 94
eil4nyqn 0:165e50fc913f 95 void MotorRun(){
eil4nyqn 0:165e50fc913f 96 char data[2]={255,0};
eil4nyqn 0:165e50fc913f 97 int i=0;
eil4nyqn 0:165e50fc913f 98 while(i<4){
eil4nyqn 0:165e50fc913f 99 i2c.write(addr[i],data,2,false);
eil4nyqn 0:165e50fc913f 100 i++;
eil4nyqn 0:165e50fc913f 101 }
eil4nyqn 0:165e50fc913f 102 }
eil4nyqn 0:165e50fc913f 103
eil4nyqn 0:165e50fc913f 104 int main() {
eil4nyqn 0:165e50fc913f 105 char MDFL[2],MDFR[2],MDRL[2],MDRR[2];
eil4nyqn 0:165e50fc913f 106 int i2cVAL=0;
eil4nyqn 0:165e50fc913f 107 int i=0;
eil4nyqn 2:3bf43044ef9f 108 //I2Cの通信速度設定
eil4nyqn 0:165e50fc913f 109 i2c.frequency(300000);
eil4nyqn 0:165e50fc913f 110 ledsReset();
eil4nyqn 2:3bf43044ef9f 111 //シリアルのボーレートを19200に設定
eil4nyqn 0:165e50fc913f 112 FEP02.baud(19200);
eil4nyqn 1:4f5a22371fff 113 /*
eil4nyqn 0:165e50fc913f 114 MotorRun();
eil4nyqn 0:165e50fc913f 115 wait(3);
eil4nyqn 1:4f5a22371fff 116 */
eil4nyqn 2:3bf43044ef9f 117 //モータの出力をリセット
eil4nyqn 0:165e50fc913f 118 MotorReset();
eil4nyqn 0:165e50fc913f 119 //FEP02.attach(&getConData);
eil4nyqn 0:165e50fc913f 120 leds[0]=1;
eil4nyqn 0:165e50fc913f 121
eil4nyqn 0:165e50fc913f 122
eil4nyqn 0:165e50fc913f 123 while(1) {
eil4nyqn 0:165e50fc913f 124 i2cVAL=0;
eil4nyqn 0:165e50fc913f 125 i=0;
eil4nyqn 2:3bf43044ef9f 126 //FEP-02の受信シーケンス
eil4nyqn 0:165e50fc913f 127 if(FEP02.getc()==255){
eil4nyqn 0:165e50fc913f 128 while(i<5){
eil4nyqn 0:165e50fc913f 129 data[i]=FEP02.getc();
eil4nyqn 0:165e50fc913f 130 i++;
eil4nyqn 0:165e50fc913f 131 }
eil4nyqn 2:3bf43044ef9f 132 //モータへの出力を算出する関数へ飛ぶ
eil4nyqn 0:165e50fc913f 133 MotorOut(MDFL,MDFR,MDRL,MDRR);
eil4nyqn 2:3bf43044ef9f 134 //各ドライバへI2Cラインを用いてデータ送信
eil4nyqn 0:165e50fc913f 135 i2cVAL += i2c.write(addr[0],MDFL,2,false);
eil4nyqn 0:165e50fc913f 136 i2cVAL += i2c.write(addr[1],MDFR,2,false);
eil4nyqn 0:165e50fc913f 137 i2cVAL += i2c.write(addr[2],MDRL,2,false);
eil4nyqn 0:165e50fc913f 138 i2cVAL += i2c.write(addr[3],MDRR,2,false);
eil4nyqn 0:165e50fc913f 139
eil4nyqn 0:165e50fc913f 140 printf("%d\r\n",i2cVAL);
eil4nyqn 0:165e50fc913f 141 printf("%d,%d,%d,%d\r\n",MDFL[0],MDFR[0],MDRL[0],MDRR[0]);
eil4nyqn 0:165e50fc913f 142 leds[0]=!leds[0];
eil4nyqn 1:4f5a22371fff 143 leds[1]=1;
eil4nyqn 2:3bf43044ef9f 144 //LEDが光ったらI2Cラインでミスが発生
eil4nyqn 1:4f5a22371fff 145 if(i2cVAL) leds[2]=1;
eil4nyqn 1:4f5a22371fff 146 else leds[2]=0;
eil4nyqn 1:4f5a22371fff 147 }
eil4nyqn 1:4f5a22371fff 148 leds[1]=0;
eil4nyqn 0:165e50fc913f 149 }
eil4nyqn 0:165e50fc913f 150 }