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@6:259deb365510, 2017-08-05 (annotated)

Committer:
tknara
Date:
Sat Aug 05 07:10:23 2017 +0000
Revision:
6:259deb365510
Parent:
5:dee9310ec990
Child:
7:ef72ec7390c2
Air cylinders are changed toggle switch

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
tknara 5:dee9310ec990 7 #define DEBUG
eil4nyqn 4:f1d2000776c6 8 MDC motor(i2c_sda,i2c_scl);
eil4nyqn 4:f1d2000776c6 9 Omni omni(4, 135);
tknara 5:dee9310ec990 10 FEP fep(PC_10,PC_11,200);
eil4nyqn 1:ba8cdae2652a 11 DigitalOut leds[4] = {PC_13,PC_14,PC_15,PA_0};
tknara 5:dee9310ec990 12 DigitalOut attack[2] = {PA_10,PC_4};
tknara 5:dee9310ec990 13 DigitalOut angle[2] = {PB_3,PB_5};
tknara 5:dee9310ec990 14 #ifdef DEBUG
tknara 5:dee9310ec990 15 Serial pc(USBTX,USBRX,115200);
tknara 5:dee9310ec990 16 #endif
WAT34 3:4cd170cdf049 17 void init()
WAT34 3:4cd170cdf049 18 {
WAT34 3:4cd170cdf049 19 for(int i = 0;i<4;i++) {
WAT34 3:4cd170cdf049 20 leds[i] = 0;
WAT34 3:4cd170cdf049 21 }
WAT34 3:4cd170cdf049 22 }
tknara 5:dee9310ec990 23 void AllActuatorStop()
tknara 5:dee9310ec990 24 {
tknara 5:dee9310ec990 25 #ifdef DEBUG
tknara 5:dee9310ec990 26 pc.printf("All actuators are stop\n");
tknara 5:dee9310ec990 27 #endif
tknara 5:dee9310ec990 28 omni.stop();
tknara 5:dee9310ec990 29 for(int j = 0; j < 4; j++) {
tknara 6:259deb365510 30 motor.write(7,j,0);
tknara 5:dee9310ec990 31 motor.write(6,j,0);
tknara 5:dee9310ec990 32 }
tknara 6:259deb365510 33 for(int i=0;i<1;i++)
tknara 6:259deb365510 34 {
tknara 6:259deb365510 35 attack[i]=0;
tknara 6:259deb365510 36 angle[i]=0;
tknara 6:259deb365510 37 }
tknara 5:dee9310ec990 38 }
UCHITAKE 0:6c83a0871cc3 39 int main()
tknara 6:259deb365510 40 {
tknara 6:259deb365510 41 bool airFlag1=0,airFlag2=0,airStatus1=0,airStatus2=0;
eil4nyqn 1:ba8cdae2652a 42 char data[10] = { 0 };
eil4nyqn 1:ba8cdae2652a 43 int i, error_val = 0, tem[2] = {0}, Button1[7] = { 0 }, Button2[6] = { 0 };
eil4nyqn 1:ba8cdae2652a 44 uint8_t fep_temp;
UCHITAKE 0:6c83a0871cc3 45 double polarVector[2];
eil4nyqn 1:ba8cdae2652a 46 double deg = 360;
tknara 5:dee9310ec990 47 double pwm = 0.0;
eil4nyqn 4:f1d2000776c6 48 for(int j = 0; j < 4; j++) {
eil4nyqn 4:f1d2000776c6 49 motor.write(7,j,0);
eil4nyqn 4:f1d2000776c6 50 }
WAT34 3:4cd170cdf049 51
UCHITAKE 0:6c83a0871cc3 52 while(1) {
tknara 5:dee9310ec990 53 fep_temp=fep.read(data,6);
tknara 5:dee9310ec990 54 if(fep_temp==0) {
WAT34 3:4cd170cdf049 55
WAT34 3:4cd170cdf049 56 #ifdef DEBUG
WAT34 3:4cd170cdf049 57 pc.printf("Recieve succeeded,%s\r\n",data);
WAT34 3:4cd170cdf049 58 #endif
WAT34 3:4cd170cdf049 59
eil4nyqn 1:ba8cdae2652a 60 error_val = 0;
eil4nyqn 1:ba8cdae2652a 61 leds[0] = 0;
eil4nyqn 1:ba8cdae2652a 62 tem[0] = data[4];
eil4nyqn 1:ba8cdae2652a 63 tem[1] = data[5];
eil4nyqn 1:ba8cdae2652a 64 for(i = 0; i < 7; i++) {
eil4nyqn 1:ba8cdae2652a 65 Button1[i] = tem[0] % 2;
eil4nyqn 1:ba8cdae2652a 66 tem[0] /= 2;
WAT34 3:4cd170cdf049 67
WAT34 3:4cd170cdf049 68 #ifdef DEBUG
WAT34 3:4cd170cdf049 69 pc.printf("%d,",Button1[i]);
WAT34 3:4cd170cdf049 70 #endif
WAT34 3:4cd170cdf049 71
eil4nyqn 1:ba8cdae2652a 72 }
tknara 5:dee9310ec990 73 pc.printf("\n");
eil4nyqn 1:ba8cdae2652a 74 for(i = 0; i < 6; i++) {
eil4nyqn 1:ba8cdae2652a 75 Button2[i] = tem[1] % 2;
eil4nyqn 1:ba8cdae2652a 76 tem[1] /= 2;
WAT34 3:4cd170cdf049 77
WAT34 3:4cd170cdf049 78 #ifdef DEBUG
WAT34 3:4cd170cdf049 79 pc.printf("%d,",Button2[i]);
WAT34 3:4cd170cdf049 80 #endif
eil4nyqn 1:ba8cdae2652a 81 }
WAT34 3:4cd170cdf049 82
WAT34 3:4cd170cdf049 83 #ifdef DEBUG
tknara 6:259deb365510 84 for(i=0;i < 4; i++) {
tknara 6:259deb365510 85 pc.printf("%d,",data[i]);
tknara 6:259deb365510 86 }
WAT34 3:4cd170cdf049 87 pc.printf("\r\n");
WAT34 3:4cd170cdf049 88 #endif
eil4nyqn 1:ba8cdae2652a 89 } else if(fep_temp==FEP_NO_RESPONSE) {
WAT34 3:4cd170cdf049 90
WAT34 3:4cd170cdf049 91 #ifdef DEBUG
WAT34 3:4cd170cdf049 92 pc.printf("No response\r\n");
WAT34 3:4cd170cdf049 93 #endif
eil4nyqn 1:ba8cdae2652a 94 leds[0] = 1;
eil4nyqn 1:ba8cdae2652a 95 continue;
eil4nyqn 1:ba8cdae2652a 96 } else {
WAT34 3:4cd170cdf049 97
WAT34 3:4cd170cdf049 98 #ifdef DEBUG
WAT34 3:4cd170cdf049 99 pc.printf("Recieve failed\r\n");
WAT34 3:4cd170cdf049 100 #endif
eil4nyqn 1:ba8cdae2652a 101 leds[0] = 1;
eil4nyqn 1:ba8cdae2652a 102 error_val++;
eil4nyqn 1:ba8cdae2652a 103 }
eil4nyqn 1:ba8cdae2652a 104 if(error_val < 4) {
tknara 5:dee9310ec990 105 polarVector[1] = 0.6;
tknara 5:dee9310ec990 106 //移動方向
eil4nyqn 4:f1d2000776c6 107 if(Button1[2] == 0 && Button1[3] == 1 && Button1[4] == 1 && Button1[5] == 1) {
eil4nyqn 4:f1d2000776c6 108 deg = 270;
eil4nyqn 4:f1d2000776c6 109 leds[1] = 0;
eil4nyqn 4:f1d2000776c6 110 }else if(Button1[2] == 1 && Button1[3] == 0 && Button1[4] == 1 && Button1[5] == 1) {
eil4nyqn 4:f1d2000776c6 111 deg = 180;
eil4nyqn 4:f1d2000776c6 112 leds[1] = 0;
eil4nyqn 4:f1d2000776c6 113 }else if(Button1[2] == 1 && Button1[3] == 1 && Button1[4] == 0 && Button1[5] == 1) {
eil4nyqn 4:f1d2000776c6 114 deg = 90;
eil4nyqn 4:f1d2000776c6 115 leds[1] = 0;
eil4nyqn 4:f1d2000776c6 116 }else if(Button1[2] == 1 && Button1[3] == 1 && Button1[4] == 1 && Button1[5] == 0) {
eil4nyqn 4:f1d2000776c6 117 deg = 0;
eil4nyqn 4:f1d2000776c6 118 leds[1] = 0;
eil4nyqn 4:f1d2000776c6 119 }else {
eil4nyqn 4:f1d2000776c6 120 deg = 360;
eil4nyqn 4:f1d2000776c6 121 polarVector[1] = 0;
eil4nyqn 4:f1d2000776c6 122 leds[1] = 1;
eil4nyqn 4:f1d2000776c6 123 }
eil4nyqn 4:f1d2000776c6 124 polarVector[0] = deg;
eil4nyqn 4:f1d2000776c6 125 if(data[2] < 50) {
eil4nyqn 4:f1d2000776c6 126 omni.computePolar(polarVector, 0.15);
eil4nyqn 4:f1d2000776c6 127 leds[2] = 1;
eil4nyqn 4:f1d2000776c6 128 leds[3] = 0;
eil4nyqn 4:f1d2000776c6 129 } else if(data[2] > 200) {
eil4nyqn 4:f1d2000776c6 130 omni.computePolar(polarVector, -0.15);
eil4nyqn 4:f1d2000776c6 131 leds[2] = 0;
eil4nyqn 4:f1d2000776c6 132 leds[3] = 1;
eil4nyqn 4:f1d2000776c6 133 } else {
eil4nyqn 1:ba8cdae2652a 134 omni.computePolar(polarVector, 0);
eil4nyqn 4:f1d2000776c6 135 leds[2] = 0;
eil4nyqn 4:f1d2000776c6 136 leds[3] = 0;
eil4nyqn 1:ba8cdae2652a 137 }
eil4nyqn 1:ba8cdae2652a 138
UCHITAKE 0:6c83a0871cc3 139 for(int j = 0; j < 4; j++) {
eil4nyqn 4:f1d2000776c6 140 motor.write(7,j,omni.getOutput(j));
UCHITAKE 0:6c83a0871cc3 141 }
tknara 5:dee9310ec990 142 //昇降
tknara 5:dee9310ec990 143 if(Button1[0] == 1 && Button1[1] == 0) {
tknara 5:dee9310ec990 144 pwm = 0.9;
tknara 5:dee9310ec990 145 } else if(Button1[0] == 0 && Button1[1] == 1) {
tknara 5:dee9310ec990 146 pwm = -0.9;
eil4nyqn 1:ba8cdae2652a 147 } else {
tknara 5:dee9310ec990 148 pwm = -0.0;
eil4nyqn 1:ba8cdae2652a 149 }
tknara 5:dee9310ec990 150 motor.write(6,0,pwm);
tknara 6:259deb365510 151 //アーム攻撃(toggle)
tknara 6:259deb365510 152 if((Button2[1]==0)&&(airFlag1 == 0))
tknara 5:dee9310ec990 153 {
tknara 6:259deb365510 154 if(airStatus1==1) {
tknara 6:259deb365510 155 attack[0]=0;
tknara 6:259deb365510 156 attack[1]=1;
tknara 6:259deb365510 157 airFlag1=1;
tknara 6:259deb365510 158 airStatus1=0;
tknara 6:259deb365510 159 }else if(airStatus1==0) {
tknara 6:259deb365510 160 attack[0]=1;
tknara 6:259deb365510 161 attack[1]=0;
tknara 6:259deb365510 162 airFlag1=1;
tknara 6:259deb365510 163 airStatus1=1;
tknara 6:259deb365510 164 }
tknara 6:259deb365510 165 }else if(Button2[1]==1){
tknara 6:259deb365510 166 airFlag1=0;
tknara 5:dee9310ec990 167 attack[0]=0;
tknara 5:dee9310ec990 168 attack[1]=0;
tknara 5:dee9310ec990 169 }
tknara 6:259deb365510 170 //アーム角度(toggle)
tknara 6:259deb365510 171 if((Button2[3]==0)&&(airFlag2 == 0))
tknara 5:dee9310ec990 172 {
tknara 6:259deb365510 173 if(airStatus2==1) {
tknara 6:259deb365510 174 angle[0]=0;
tknara 6:259deb365510 175 angle[1]=1;
tknara 6:259deb365510 176 airFlag2=1;
tknara 6:259deb365510 177 airStatus2=0;
tknara 6:259deb365510 178 }else if(airStatus2==0) {
tknara 6:259deb365510 179 angle[0]=1;
tknara 6:259deb365510 180 angle[1]=0;
tknara 6:259deb365510 181 airFlag2=1;
tknara 6:259deb365510 182 airStatus2=1;
tknara 6:259deb365510 183 }
tknara 6:259deb365510 184 }else if(Button2[3]==1){
tknara 6:259deb365510 185 airFlag2=0;
tknara 5:dee9310ec990 186 angle[0]=0;
tknara 5:dee9310ec990 187 angle[1]=0;
tknara 5:dee9310ec990 188 }
tknara 6:259deb365510 189 #ifdef DEBUG
tknara 6:259deb365510 190 printf("Status of airFlags %d,%d\r\n",airStatus1,airStatus2);
tknara 6:259deb365510 191 #endif
tknara 5:dee9310ec990 192 } else if((fep_temp==FEP_NO_RESPONSE)&&(error_val > 4)){
tknara 5:dee9310ec990 193 AllActuatorStop();
eil4nyqn 1:ba8cdae2652a 194 } else {
tknara 5:dee9310ec990 195 AllActuatorStop();
UCHITAKE 0:6c83a0871cc3 196 }
UCHITAKE 0:6c83a0871cc3 197 }
UCHITAKE 0:6c83a0871cc3 198 }