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@8:244c057d195c, 2017-09-01 (annotated)

Committer:
tknara
Date:
Fri Sep 01 06:38:15 2017 +0000
Revision:
8:244c057d195c
Parent:
7:ef72ec7390c2
Child:
9:8f9607783d2d
support MDC ver3.0;

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