NagaokaRoboticsClub_mbedTeam / Mbed 2 deprecated MecanumWheel3

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

Dependencies:   mbed

main.cpp@1:4f5a22371fff, 2016-05-06 (annotated)

Committer:
eil4nyqn
Date:
Fri May 06 07:49:34 2016 +0000
Revision:
1:4f5a22371fff
Parent:
0:165e50fc913f
Child:
2:3bf43044ef9f
2016/05/06

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 0:165e50fc913f 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 0:165e50fc913f 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 0:165e50fc913f 43
eil4nyqn 0:165e50fc913f 44 dig = data[0]+data[1]*128;
eil4nyqn 0:165e50fc913f 45 rad = (dig/180.0)*PI;
eil4nyqn 0:165e50fc913f 46 if(dig==360){
eil4nyqn 0:165e50fc913f 47 while(n<4){
eil4nyqn 0:165e50fc913f 48 prePwmDuty[n]=0;
eil4nyqn 0:165e50fc913f 49 n++;
eil4nyqn 0:165e50fc913f 50 }
eil4nyqn 0:165e50fc913f 51 }else{
eil4nyqn 1:4f5a22371fff 52 prePwmDuty[0]=1.41421356*sin(rad+qPI);//*conratio;
eil4nyqn 1:4f5a22371fff 53 prePwmDuty[1]=1.41421356*sin(rad-qPI);//*conratio;
eil4nyqn 0:165e50fc913f 54 prePwmDuty[2]=prePwmDuty[1];
eil4nyqn 0:165e50fc913f 55 prePwmDuty[3]=prePwmDuty[0];
eil4nyqn 0:165e50fc913f 56 }
eil4nyqn 0:165e50fc913f 57
eil4nyqn 0:165e50fc913f 58 turn = (data[2]-63)/64.0;
eil4nyqn 1:4f5a22371fff 59 //turn = 0;
eil4nyqn 0:165e50fc913f 60
eil4nyqn 1:4f5a22371fff 61 prePwmDuty[0]+=turn;//*conratio;//*ratio;
eil4nyqn 1:4f5a22371fff 62 prePwmDuty[1]-=turn;//*conratio;//*ratio;
eil4nyqn 1:4f5a22371fff 63 prePwmDuty[2]+=turn;//*conratio;//*ratio;
eil4nyqn 1:4f5a22371fff 64 prePwmDuty[3]-=turn;//*conratio;//*ratio;
eil4nyqn 0:165e50fc913f 65
eil4nyqn 0:165e50fc913f 66 while(i<4){
eil4nyqn 0:165e50fc913f 67 if(prePwmDuty[i]<-1.0) prePwmDuty[i]=-1.0;
eil4nyqn 0:165e50fc913f 68 else if(prePwmDuty[i]>1.0) prePwmDuty[i]=1.0;
eil4nyqn 0:165e50fc913f 69 PwmDuty[i]=(fabs(prePwmDuty[i]+1.0))/2;
eil4nyqn 0:165e50fc913f 70 i++;
eil4nyqn 0:165e50fc913f 71 }
eil4nyqn 0:165e50fc913f 72
eil4nyqn 0:165e50fc913f 73 MDFL[0] = PwmDuty[0]*255;
eil4nyqn 0:165e50fc913f 74 MDFR[0] = PwmDuty[1]*255;
eil4nyqn 0:165e50fc913f 75 MDRL[0] = PwmDuty[2]*255;
eil4nyqn 0:165e50fc913f 76 MDRR[0] = PwmDuty[3]*255;
eil4nyqn 0:165e50fc913f 77
eil4nyqn 0:165e50fc913f 78 //printf("%f,%f,%f,%f\r\n",PwmDuty[0],PwmDuty[1],PwmDuty[2],PwmDuty[3]);
eil4nyqn 0:165e50fc913f 79 }
eil4nyqn 0:165e50fc913f 80
eil4nyqn 0:165e50fc913f 81 void MotorReset(){
eil4nyqn 0:165e50fc913f 82 char data[2]={127,0};
eil4nyqn 0:165e50fc913f 83 int i=0;
eil4nyqn 0:165e50fc913f 84 while(i<4){
eil4nyqn 0:165e50fc913f 85 i2c.write(addr[i],data,2,false);
eil4nyqn 0:165e50fc913f 86 i++;
eil4nyqn 0:165e50fc913f 87 }
eil4nyqn 0:165e50fc913f 88 }
eil4nyqn 0:165e50fc913f 89
eil4nyqn 0:165e50fc913f 90 void MotorRun(){
eil4nyqn 0:165e50fc913f 91 char data[2]={255,0};
eil4nyqn 0:165e50fc913f 92 int i=0;
eil4nyqn 0:165e50fc913f 93 while(i<4){
eil4nyqn 0:165e50fc913f 94 i2c.write(addr[i],data,2,false);
eil4nyqn 0:165e50fc913f 95 i++;
eil4nyqn 0:165e50fc913f 96 }
eil4nyqn 0:165e50fc913f 97 }
eil4nyqn 0:165e50fc913f 98
eil4nyqn 0:165e50fc913f 99 int main() {
eil4nyqn 0:165e50fc913f 100 char MDFL[2],MDFR[2],MDRL[2],MDRR[2];
eil4nyqn 0:165e50fc913f 101 int i2cVAL=0;
eil4nyqn 0:165e50fc913f 102 int i=0;
eil4nyqn 0:165e50fc913f 103
eil4nyqn 0:165e50fc913f 104 i2c.frequency(300000);
eil4nyqn 0:165e50fc913f 105 ledsReset();
eil4nyqn 0:165e50fc913f 106 FEP02.baud(19200);
eil4nyqn 1:4f5a22371fff 107 /*
eil4nyqn 0:165e50fc913f 108 MotorRun();
eil4nyqn 0:165e50fc913f 109 wait(3);
eil4nyqn 1:4f5a22371fff 110 */
eil4nyqn 0:165e50fc913f 111 MotorReset();
eil4nyqn 0:165e50fc913f 112 //FEP02.attach(&getConData);
eil4nyqn 0:165e50fc913f 113 leds[0]=1;
eil4nyqn 0:165e50fc913f 114
eil4nyqn 0:165e50fc913f 115
eil4nyqn 0:165e50fc913f 116 while(1) {
eil4nyqn 0:165e50fc913f 117 i2cVAL=0;
eil4nyqn 0:165e50fc913f 118 i=0;
eil4nyqn 0:165e50fc913f 119 if(FEP02.getc()==255){
eil4nyqn 0:165e50fc913f 120 while(i<5){
eil4nyqn 0:165e50fc913f 121 data[i]=FEP02.getc();
eil4nyqn 0:165e50fc913f 122 i++;
eil4nyqn 0:165e50fc913f 123 }
eil4nyqn 0:165e50fc913f 124 MotorOut(MDFL,MDFR,MDRL,MDRR);
eil4nyqn 0:165e50fc913f 125
eil4nyqn 0:165e50fc913f 126 i2cVAL += i2c.write(addr[0],MDFL,2,false);
eil4nyqn 0:165e50fc913f 127 i2cVAL += i2c.write(addr[1],MDFR,2,false);
eil4nyqn 0:165e50fc913f 128 i2cVAL += i2c.write(addr[2],MDRL,2,false);
eil4nyqn 0:165e50fc913f 129 i2cVAL += i2c.write(addr[3],MDRR,2,false);
eil4nyqn 0:165e50fc913f 130
eil4nyqn 0:165e50fc913f 131 printf("%d\r\n",i2cVAL);
eil4nyqn 0:165e50fc913f 132 printf("%d,%d,%d,%d\r\n",MDFL[0],MDFR[0],MDRL[0],MDRR[0]);
eil4nyqn 0:165e50fc913f 133 leds[0]=!leds[0];
eil4nyqn 1:4f5a22371fff 134 leds[1]=1;
eil4nyqn 1:4f5a22371fff 135 if(i2cVAL) leds[2]=1;
eil4nyqn 1:4f5a22371fff 136 else leds[2]=0;
eil4nyqn 1:4f5a22371fff 137 }
eil4nyqn 1:4f5a22371fff 138 leds[1]=0;
eil4nyqn 0:165e50fc913f 139 }
eil4nyqn 0:165e50fc913f 140 }