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@7:ef72ec7390c2, 2017-08-31 (annotated)

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