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@5:dee9310ec990, 2017-08-05 (annotated)

Committer:
tknara
Date:
Sat Aug 05 05:57:53 2017 +0000
Revision:
5:dee9310ec990
Parent:
4:f1d2000776c6
Child:
6:259deb365510
Used at OC

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 5:dee9310ec990 30 motor.write(7,j,omni.getOutput(j));
tknara 5:dee9310ec990 31 motor.write(6,j,0);
tknara 5:dee9310ec990 32 }
tknara 5:dee9310ec990 33 attack[0]=1;
tknara 5:dee9310ec990 34 attack[1]=0;
tknara 5:dee9310ec990 35 angle[0]=0;
tknara 5:dee9310ec990 36 angle[1]=1;
tknara 5:dee9310ec990 37 }
UCHITAKE 0:6c83a0871cc3 38 int main()
UCHITAKE 0:6c83a0871cc3 39 {
eil4nyqn 1:ba8cdae2652a 40 char data[10] = { 0 };
eil4nyqn 1:ba8cdae2652a 41 int i, error_val = 0, tem[2] = {0}, Button1[7] = { 0 }, Button2[6] = { 0 };
eil4nyqn 1:ba8cdae2652a 42 uint8_t fep_temp;
UCHITAKE 0:6c83a0871cc3 43 double polarVector[2];
eil4nyqn 1:ba8cdae2652a 44 double deg = 360;
tknara 5:dee9310ec990 45 double pwm = 0.0;
eil4nyqn 4:f1d2000776c6 46 for(int j = 0; j < 4; j++) {
eil4nyqn 4:f1d2000776c6 47 motor.write(7,j,0);
eil4nyqn 4:f1d2000776c6 48 }
WAT34 3:4cd170cdf049 49
UCHITAKE 0:6c83a0871cc3 50 while(1) {
tknara 5:dee9310ec990 51 fep_temp=fep.read(data,6);
tknara 5:dee9310ec990 52 if(fep_temp==0) {
WAT34 3:4cd170cdf049 53
WAT34 3:4cd170cdf049 54 #ifdef DEBUG
WAT34 3:4cd170cdf049 55 pc.printf("Recieve succeeded,%s\r\n",data);
WAT34 3:4cd170cdf049 56 #endif
WAT34 3:4cd170cdf049 57
eil4nyqn 1:ba8cdae2652a 58 error_val = 0;
eil4nyqn 1:ba8cdae2652a 59 leds[0] = 0;
eil4nyqn 1:ba8cdae2652a 60 tem[0] = data[4];
eil4nyqn 1:ba8cdae2652a 61 tem[1] = data[5];
eil4nyqn 1:ba8cdae2652a 62 for(i = 0; i < 7; i++) {
eil4nyqn 1:ba8cdae2652a 63 Button1[i] = tem[0] % 2;
eil4nyqn 1:ba8cdae2652a 64 tem[0] /= 2;
WAT34 3:4cd170cdf049 65
WAT34 3:4cd170cdf049 66 #ifdef DEBUG
WAT34 3:4cd170cdf049 67 pc.printf("%d,",Button1[i]);
WAT34 3:4cd170cdf049 68 #endif
WAT34 3:4cd170cdf049 69
eil4nyqn 1:ba8cdae2652a 70 }
tknara 5:dee9310ec990 71 pc.printf("\n");
eil4nyqn 1:ba8cdae2652a 72 for(i = 0; i < 6; i++) {
eil4nyqn 1:ba8cdae2652a 73 Button2[i] = tem[1] % 2;
eil4nyqn 1:ba8cdae2652a 74 tem[1] /= 2;
WAT34 3:4cd170cdf049 75
WAT34 3:4cd170cdf049 76 #ifdef DEBUG
WAT34 3:4cd170cdf049 77 pc.printf("%d,",Button2[i]);
WAT34 3:4cd170cdf049 78 #endif
eil4nyqn 1:ba8cdae2652a 79 }
WAT34 3:4cd170cdf049 80
WAT34 3:4cd170cdf049 81 #ifdef DEBUG
WAT34 3:4cd170cdf049 82 pc.printf("\r\n");
WAT34 3:4cd170cdf049 83 #endif
eil4nyqn 1:ba8cdae2652a 84 } else if(fep_temp==FEP_NO_RESPONSE) {
WAT34 3:4cd170cdf049 85
WAT34 3:4cd170cdf049 86 #ifdef DEBUG
WAT34 3:4cd170cdf049 87 pc.printf("No response\r\n");
WAT34 3:4cd170cdf049 88 #endif
eil4nyqn 1:ba8cdae2652a 89 leds[0] = 1;
eil4nyqn 1:ba8cdae2652a 90 continue;
eil4nyqn 1:ba8cdae2652a 91 } else {
WAT34 3:4cd170cdf049 92
WAT34 3:4cd170cdf049 93 #ifdef DEBUG
WAT34 3:4cd170cdf049 94 pc.printf("Recieve failed\r\n");
WAT34 3:4cd170cdf049 95 #endif
eil4nyqn 1:ba8cdae2652a 96 leds[0] = 1;
eil4nyqn 1:ba8cdae2652a 97 error_val++;
eil4nyqn 1:ba8cdae2652a 98 }
eil4nyqn 1:ba8cdae2652a 99 if(error_val < 4) {
tknara 5:dee9310ec990 100 #ifdef DEBUG
tknara 5:dee9310ec990 101 //for(i = 0;i<7;i++) printf("%d\n",Button1[i]);
tknara 5:dee9310ec990 102 #endif
tknara 5:dee9310ec990 103 polarVector[1] = 0.6;
tknara 5:dee9310ec990 104 //移動方向
eil4nyqn 4:f1d2000776c6 105 if(Button1[2] == 0 && Button1[3] == 1 && Button1[4] == 1 && Button1[5] == 1) {
eil4nyqn 4:f1d2000776c6 106 deg = 270;
eil4nyqn 4:f1d2000776c6 107 leds[1] = 0;
eil4nyqn 4:f1d2000776c6 108 }else if(Button1[2] == 1 && Button1[3] == 0 && Button1[4] == 1 && Button1[5] == 1) {
eil4nyqn 4:f1d2000776c6 109 deg = 180;
eil4nyqn 4:f1d2000776c6 110 leds[1] = 0;
eil4nyqn 4:f1d2000776c6 111 }else if(Button1[2] == 1 && Button1[3] == 1 && Button1[4] == 0 && Button1[5] == 1) {
eil4nyqn 4:f1d2000776c6 112 deg = 90;
eil4nyqn 4:f1d2000776c6 113 leds[1] = 0;
eil4nyqn 4:f1d2000776c6 114 }else if(Button1[2] == 1 && Button1[3] == 1 && Button1[4] == 1 && Button1[5] == 0) {
eil4nyqn 4:f1d2000776c6 115 deg = 0;
eil4nyqn 4:f1d2000776c6 116 leds[1] = 0;
eil4nyqn 4:f1d2000776c6 117 }else {
eil4nyqn 4:f1d2000776c6 118 deg = 360;
eil4nyqn 4:f1d2000776c6 119 polarVector[1] = 0;
eil4nyqn 4:f1d2000776c6 120 leds[1] = 1;
eil4nyqn 4:f1d2000776c6 121 }
eil4nyqn 4:f1d2000776c6 122 polarVector[0] = deg;
eil4nyqn 4:f1d2000776c6 123 if(data[2] < 50) {
eil4nyqn 4:f1d2000776c6 124 omni.computePolar(polarVector, 0.15);
eil4nyqn 4:f1d2000776c6 125 leds[2] = 1;
eil4nyqn 4:f1d2000776c6 126 leds[3] = 0;
eil4nyqn 4:f1d2000776c6 127 } else if(data[2] > 200) {
eil4nyqn 4:f1d2000776c6 128 omni.computePolar(polarVector, -0.15);
eil4nyqn 4:f1d2000776c6 129 leds[2] = 0;
eil4nyqn 4:f1d2000776c6 130 leds[3] = 1;
eil4nyqn 4:f1d2000776c6 131 } else {
eil4nyqn 1:ba8cdae2652a 132 omni.computePolar(polarVector, 0);
eil4nyqn 4:f1d2000776c6 133 leds[2] = 0;
eil4nyqn 4:f1d2000776c6 134 leds[3] = 0;
eil4nyqn 1:ba8cdae2652a 135 }
eil4nyqn 1:ba8cdae2652a 136
UCHITAKE 0:6c83a0871cc3 137 for(int j = 0; j < 4; j++) {
eil4nyqn 4:f1d2000776c6 138 motor.write(7,j,omni.getOutput(j));
UCHITAKE 0:6c83a0871cc3 139 }
tknara 5:dee9310ec990 140 //昇降
tknara 5:dee9310ec990 141 if(Button1[0] == 1 && Button1[1] == 0) {
tknara 5:dee9310ec990 142 pwm = 0.9;
tknara 5:dee9310ec990 143 } else if(Button1[0] == 0 && Button1[1] == 1) {
tknara 5:dee9310ec990 144 pwm = -0.9;
eil4nyqn 1:ba8cdae2652a 145 } else {
tknara 5:dee9310ec990 146 pwm = -0.0;
eil4nyqn 1:ba8cdae2652a 147 }
tknara 5:dee9310ec990 148 motor.write(6,0,pwm);
tknara 5:dee9310ec990 149 //アーム攻撃
tknara 5:dee9310ec990 150 if((Button2[0]==1)&&(Button2[1]==0))
tknara 5:dee9310ec990 151 {
tknara 5:dee9310ec990 152 attack[0]=0;
tknara 5:dee9310ec990 153 attack[1]=1;
tknara 5:dee9310ec990 154 }else if((Button2[0]==0)&&(Button2[1]==1)){
tknara 5:dee9310ec990 155 attack[0]=1;
tknara 5:dee9310ec990 156 attack[1]=0;
tknara 5:dee9310ec990 157 }else{
tknara 5:dee9310ec990 158 attack[0]=0;
tknara 5:dee9310ec990 159 attack[1]=0;
tknara 5:dee9310ec990 160 }
tknara 5:dee9310ec990 161 //アーム角度
tknara 5:dee9310ec990 162 if((Button2[2]==1)&&(Button2[3]==0))
tknara 5:dee9310ec990 163 {
tknara 5:dee9310ec990 164 angle[0]=0;
tknara 5:dee9310ec990 165 angle[1]=1;
tknara 5:dee9310ec990 166 }else if((Button2[2]==0)&&Button2[3]==1){
tknara 5:dee9310ec990 167 angle[0]=1;
tknara 5:dee9310ec990 168 angle[1]=0;
tknara 5:dee9310ec990 169 }else{
tknara 5:dee9310ec990 170 angle[0]=0;
tknara 5:dee9310ec990 171 angle[1]=0;
tknara 5:dee9310ec990 172 }
tknara 5:dee9310ec990 173 } else if((fep_temp==FEP_NO_RESPONSE)&&(error_val > 4)){
tknara 5:dee9310ec990 174 AllActuatorStop();
eil4nyqn 1:ba8cdae2652a 175 } else {
tknara 5:dee9310ec990 176 AllActuatorStop();
UCHITAKE 0:6c83a0871cc3 177 }
UCHITAKE 0:6c83a0871cc3 178 }
UCHITAKE 0:6c83a0871cc3 179 }