2016/05/06 交ロボの移植プログラム 移動only
Dependencies: mbed
main.cpp@0:165e50fc913f, 2016-04-12 (annotated)
- Committer:
- eil4nyqn
- Date:
- Tue Apr 12 12:28:01 2016 +0000
- Revision:
- 0:165e50fc913f
- Child:
- 1:4f5a22371fff
20160412
Who changed what in which revision?
User | Revision | Line number | New 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 | 0:165e50fc913f | 52 | prePwmDuty[0]=1.41421356*sin(rad+qPI)*conratio; |
eil4nyqn | 0:165e50fc913f | 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 | 0:165e50fc913f | 59 | turn = 0; |
eil4nyqn | 0:165e50fc913f | 60 | |
eil4nyqn | 0:165e50fc913f | 61 | prePwmDuty[0]+=turn*ratio; |
eil4nyqn | 0:165e50fc913f | 62 | prePwmDuty[1]+=turn*ratio; |
eil4nyqn | 0:165e50fc913f | 63 | prePwmDuty[2]-=turn*ratio; |
eil4nyqn | 0:165e50fc913f | 64 | prePwmDuty[3]-=turn*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 | 0:165e50fc913f | 107 | MotorRun(); |
eil4nyqn | 0:165e50fc913f | 108 | wait(3); |
eil4nyqn | 0:165e50fc913f | 109 | MotorReset(); |
eil4nyqn | 0:165e50fc913f | 110 | //FEP02.attach(&getConData); |
eil4nyqn | 0:165e50fc913f | 111 | leds[0]=1; |
eil4nyqn | 0:165e50fc913f | 112 | |
eil4nyqn | 0:165e50fc913f | 113 | |
eil4nyqn | 0:165e50fc913f | 114 | while(1) { |
eil4nyqn | 0:165e50fc913f | 115 | i2cVAL=0; |
eil4nyqn | 0:165e50fc913f | 116 | i=0; |
eil4nyqn | 0:165e50fc913f | 117 | if(FEP02.getc()==255){ |
eil4nyqn | 0:165e50fc913f | 118 | while(i<5){ |
eil4nyqn | 0:165e50fc913f | 119 | data[i]=FEP02.getc(); |
eil4nyqn | 0:165e50fc913f | 120 | i++; |
eil4nyqn | 0:165e50fc913f | 121 | } |
eil4nyqn | 0:165e50fc913f | 122 | MotorOut(MDFL,MDFR,MDRL,MDRR); |
eil4nyqn | 0:165e50fc913f | 123 | |
eil4nyqn | 0:165e50fc913f | 124 | i2cVAL += i2c.write(addr[0],MDFL,2,false); |
eil4nyqn | 0:165e50fc913f | 125 | i2cVAL += i2c.write(addr[1],MDFR,2,false); |
eil4nyqn | 0:165e50fc913f | 126 | i2cVAL += i2c.write(addr[2],MDRL,2,false); |
eil4nyqn | 0:165e50fc913f | 127 | i2cVAL += i2c.write(addr[3],MDRR,2,false); |
eil4nyqn | 0:165e50fc913f | 128 | |
eil4nyqn | 0:165e50fc913f | 129 | printf("%d\r\n",i2cVAL); |
eil4nyqn | 0:165e50fc913f | 130 | printf("%d,%d,%d,%d\r\n",MDFL[0],MDFR[0],MDRL[0],MDRR[0]); |
eil4nyqn | 0:165e50fc913f | 131 | leds[0]=!leds[0]; |
eil4nyqn | 0:165e50fc913f | 132 | } |
eil4nyqn | 0:165e50fc913f | 133 | } |
eil4nyqn | 0:165e50fc913f | 134 | } |