use for experiment before the demonstration at open-campus
Dependencies: FEP ikarashiMDC omni PID R1370
Fork of omni_sample by
main.cpp@2:b9297c2b2632, 2017-07-13 (annotated)
- Committer:
- WAT34
- Date:
- Thu Jul 13 12:02:06 2017 +0000
- Revision:
- 2:b9297c2b2632
- Parent:
- 1:ba8cdae2652a
- Child:
- 3:4cd170cdf049
fixed i hope.... at least i think
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
UCHITAKE | 0:6c83a0871cc3 | 1 | #include "mbed.h" |
UCHITAKE | 0:6c83a0871cc3 | 2 | #include "omni.h" |
UCHITAKE | 0:6c83a0871cc3 | 3 | #include "MotorDriverController.h" |
UCHITAKE | 0:6c83a0871cc3 | 4 | #include "pin_config.h" |
eil4nyqn | 1:ba8cdae2652a | 5 | #include "FEP.h" |
UCHITAKE | 0:6c83a0871cc3 | 6 | |
WAT34 | 2:b9297c2b2632 | 7 | MDC motor[4]; |
UCHITAKE | 0:6c83a0871cc3 | 8 | Omni omni(4, 45); |
WAT34 | 2:b9297c2b2632 | 9 | FEP fep(PA_0,PA_1,115200); |
eil4nyqn | 1:ba8cdae2652a | 10 | DigitalOut leds[4] = {PC_13,PC_14,PC_15,PA_0}; |
eil4nyqn | 1:ba8cdae2652a | 11 | PwmOut pwm(PA_6); |
UCHITAKE | 0:6c83a0871cc3 | 12 | |
UCHITAKE | 0:6c83a0871cc3 | 13 | int main() |
UCHITAKE | 0:6c83a0871cc3 | 14 | { |
eil4nyqn | 1:ba8cdae2652a | 15 | char data[10] = { 0 }; |
eil4nyqn | 1:ba8cdae2652a | 16 | int i, error_val = 0, tem[2] = {0}, Button1[7] = { 0 }, Button2[6] = { 0 }; |
eil4nyqn | 1:ba8cdae2652a | 17 | uint8_t fep_temp; |
UCHITAKE | 0:6c83a0871cc3 | 18 | double polarVector[2]; |
eil4nyqn | 1:ba8cdae2652a | 19 | double Xstick,Ystick; |
eil4nyqn | 1:ba8cdae2652a | 20 | double radian; |
eil4nyqn | 1:ba8cdae2652a | 21 | double deg = 360; |
eil4nyqn | 1:ba8cdae2652a | 22 | pwm = 0.5; |
eil4nyqn | 1:ba8cdae2652a | 23 | for(i = 0;i<4;i++) { |
eil4nyqn | 1:ba8cdae2652a | 24 | leds[i] = 0; |
eil4nyqn | 1:ba8cdae2652a | 25 | } |
eil4nyqn | 1:ba8cdae2652a | 26 | for(int j = 0; j < 4; j++) { |
WAT34 | 2:b9297c2b2632 | 27 | motor[j].setAddr(7, j); |
eil4nyqn | 1:ba8cdae2652a | 28 | } |
UCHITAKE | 0:6c83a0871cc3 | 29 | while(1) { |
eil4nyqn | 1:ba8cdae2652a | 30 | fep_temp=fep.read_txt(data,6); |
eil4nyqn | 1:ba8cdae2652a | 31 | if(fep_temp==FEP_RX_SUCCESS) { |
eil4nyqn | 1:ba8cdae2652a | 32 | // pc.printf("Recieve succeeded,%s\r\n",data); |
eil4nyqn | 1:ba8cdae2652a | 33 | error_val = 0; |
eil4nyqn | 1:ba8cdae2652a | 34 | leds[0] = 0; |
eil4nyqn | 1:ba8cdae2652a | 35 | tem[0] = data[4]; |
eil4nyqn | 1:ba8cdae2652a | 36 | tem[1] = data[5]; |
eil4nyqn | 1:ba8cdae2652a | 37 | for(i = 0; i < 7; i++) { |
eil4nyqn | 1:ba8cdae2652a | 38 | Button1[i] = tem[0] % 2; |
eil4nyqn | 1:ba8cdae2652a | 39 | tem[0] /= 2; |
eil4nyqn | 1:ba8cdae2652a | 40 | // pc.printf("%d,",Button1[i]); |
eil4nyqn | 1:ba8cdae2652a | 41 | } |
eil4nyqn | 1:ba8cdae2652a | 42 | for(i = 0; i < 6; i++) { |
eil4nyqn | 1:ba8cdae2652a | 43 | Button2[i] = tem[1] % 2; |
eil4nyqn | 1:ba8cdae2652a | 44 | tem[1] /= 2; |
eil4nyqn | 1:ba8cdae2652a | 45 | // pc.printf("%d,",Button2[i]); |
eil4nyqn | 1:ba8cdae2652a | 46 | } |
eil4nyqn | 1:ba8cdae2652a | 47 | // pc.printf("\r\n"); |
eil4nyqn | 1:ba8cdae2652a | 48 | } else if(fep_temp==FEP_NO_RESPONSE) { |
eil4nyqn | 1:ba8cdae2652a | 49 | // pc.printf("No response\r\n"); |
eil4nyqn | 1:ba8cdae2652a | 50 | leds[0] = 1; |
eil4nyqn | 1:ba8cdae2652a | 51 | continue; |
eil4nyqn | 1:ba8cdae2652a | 52 | } else { |
eil4nyqn | 1:ba8cdae2652a | 53 | // pc.printf("Recieve failed\r\n"); |
eil4nyqn | 1:ba8cdae2652a | 54 | leds[0] = 1; |
eil4nyqn | 1:ba8cdae2652a | 55 | error_val++; |
eil4nyqn | 1:ba8cdae2652a | 56 | } |
eil4nyqn | 1:ba8cdae2652a | 57 | if(error_val < 4) { |
eil4nyqn | 1:ba8cdae2652a | 58 | if (data[0]>100 && data[0]<150 && data[1]>100 && data[1]<150) { |
eil4nyqn | 1:ba8cdae2652a | 59 | omni.stop(); |
eil4nyqn | 1:ba8cdae2652a | 60 | leds[2] = 1; |
eil4nyqn | 1:ba8cdae2652a | 61 | } else { |
eil4nyqn | 1:ba8cdae2652a | 62 | leds[2] = 0; |
eil4nyqn | 1:ba8cdae2652a | 63 | Xstick = (double)(data[0]/255.0)*2.0-1.0; |
eil4nyqn | 1:ba8cdae2652a | 64 | Ystick = (double)(data[1]/255.0)*2.0-1.0; |
eil4nyqn | 1:ba8cdae2652a | 65 | radian = atan2(-(Ystick),-(Xstick))+PI/2.0; |
eil4nyqn | 1:ba8cdae2652a | 66 | deg = (radian*180/PI-20); |
eil4nyqn | 1:ba8cdae2652a | 67 | polarVector[0] = deg; |
eil4nyqn | 1:ba8cdae2652a | 68 | polarVector[1] = 0.5; |
eil4nyqn | 1:ba8cdae2652a | 69 | |
eil4nyqn | 1:ba8cdae2652a | 70 | omni.computePolar(polarVector, 0); |
eil4nyqn | 1:ba8cdae2652a | 71 | } |
eil4nyqn | 1:ba8cdae2652a | 72 | |
UCHITAKE | 0:6c83a0871cc3 | 73 | for(int j = 0; j < 4; j++) { |
WAT34 | 2:b9297c2b2632 | 74 | motor[j].write(omni.getOutput(j)); |
UCHITAKE | 0:6c83a0871cc3 | 75 | } |
eil4nyqn | 1:ba8cdae2652a | 76 | if(Button2[0] == 1 && Button2[1] == 0) { |
eil4nyqn | 1:ba8cdae2652a | 77 | pwm = 0.2; |
eil4nyqn | 1:ba8cdae2652a | 78 | } else if(Button2[0] == 0 && Button2[1] == 1) { |
eil4nyqn | 1:ba8cdae2652a | 79 | pwm = 0.8; |
eil4nyqn | 1:ba8cdae2652a | 80 | } else { |
eil4nyqn | 1:ba8cdae2652a | 81 | pwm = 0.5; |
eil4nyqn | 1:ba8cdae2652a | 82 | } |
eil4nyqn | 1:ba8cdae2652a | 83 | } else { |
eil4nyqn | 1:ba8cdae2652a | 84 | omni.stop(); |
eil4nyqn | 1:ba8cdae2652a | 85 | for(int j = 0; j < 4; j++) { |
WAT34 | 2:b9297c2b2632 | 86 | motor[j].write(omni.getOutput(j)); |
eil4nyqn | 1:ba8cdae2652a | 87 | } |
eil4nyqn | 1:ba8cdae2652a | 88 | pwm = 0.5; |
UCHITAKE | 0:6c83a0871cc3 | 89 | } |
UCHITAKE | 0:6c83a0871cc3 | 90 | } |
UCHITAKE | 0:6c83a0871cc3 | 91 | } |