NagaokaRoboticsClub_mbedTeam / Mbed OS quadOmni_yanagi

use for experiment before the demonstration at open-campus

Dependencies:   FEP ikarashiMDC omni PID R1370

Fork of omni_sample by NagaokaRoboticsClub_mbedTeam

main.cpp@1:ba8cdae2652a, 2017-07-13 (annotated)

Committer:
eil4nyqn
Date:
Thu Jul 13 11:33:01 2017 +0000
Revision:
1:ba8cdae2652a
Parent:
0:6c83a0871cc3
Child:
2:b9297c2b2632
first commit not work

Who changed what in which revision?

UserRevisionLine numberNew 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
UCHITAKE 0:6c83a0871cc3 7 MDC motor(i2c_sda, i2c_scl);
UCHITAKE 0:6c83a0871cc3 8 Omni omni(4, 45);
eil4nyqn 1:ba8cdae2652a 9 FEP fep(PB_10,PB_11);
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++) {
eil4nyqn 1:ba8cdae2652a 27 motor.write(7, j, 0);
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++) {
UCHITAKE 0:6c83a0871cc3 74 motor.write(7, j, 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++) {
eil4nyqn 1:ba8cdae2652a 86 motor.write(7, j, 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 }