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

Committer:
tknara
Date:
Fri Sep 01 08:19:08 2017 +0000
Revision:
9:8f9607783d2d
Parent:
8:244c057d195c
Child:
10:73148221684e
manual mode

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"
tknara 9:8f9607783d2d 6 #include "controller.h"
UCHITAKE 0:6c83a0871cc3 7
tknara 8:244c057d195c 8 #define stickrange 0.25
tknara 5:dee9310ec990 9 #define DEBUG
tknara 7:ef72ec7390c2 10 Omni omni(4);
tknara 9:8f9607783d2d 11 Serial RS485(RS485_TX,RS485_RX,38400);
tknara 9:8f9607783d2d 12 Serial pc(USBTX,USBRX,115200);
tknara 9:8f9607783d2d 13 Controller con(FEP_TX,FEP_RX,200);
tknara 8:244c057d195c 14 DigitalOut RS485control(D2);
eil4nyqn 1:ba8cdae2652a 15 DigitalOut leds[4] = {PC_13,PC_14,PC_15,PA_0};
tknara 5:dee9310ec990 16 DigitalOut attack[2] = {PA_10,PC_4};
tknara 5:dee9310ec990 17 DigitalOut angle[2] = {PB_3,PB_5};
tknara 9:8f9607783d2d 18
tknara 8:244c057d195c 19
tknara 7:ef72ec7390c2 20 ikarashiMDC wheels[] {
tknara 8:244c057d195c 21 ikarashiMDC(&RS485control,0,0,SM,&RS485),
tknara 8:244c057d195c 22 ikarashiMDC(&RS485control,1,3,SM,&RS485),
tknara 8:244c057d195c 23 ikarashiMDC(&RS485control,1,0,SM,&RS485),
tknara 8:244c057d195c 24 ikarashiMDC(&RS485control,0,3,SM,&RS485)
tknara 7:ef72ec7390c2 25 };
tknara 7:ef72ec7390c2 26 ikarashiMDC lift[] {
tknara 9:8f9607783d2d 27 ikarashiMDC(&RS485control,1,1,SM,&RS485)
tknara 7:ef72ec7390c2 28 };
WAT34 3:4cd170cdf049 29 void init()
WAT34 3:4cd170cdf049 30 {
tknara 8:244c057d195c 31 int i;
tknara 8:244c057d195c 32 for(i = 0;i < 4;i++) {
tknara 8:244c057d195c 33 wheels[i].braking = true;
tknara 8:244c057d195c 34 }
tknara 9:8f9607783d2d 35 lift[0].braking = true;
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;
tknara 9:8f9607783d2d 55 int error_val = 0;
eil4nyqn 1:ba8cdae2652a 56 uint8_t fep_temp;
tknara 5:dee9310ec990 57 double pwm = 0.0;
tknara 8:244c057d195c 58 init();
UCHITAKE 0:6c83a0871cc3 59 while(1) {
tknara 9:8f9607783d2d 60 if(con.receiveState()==0) {
eil4nyqn 1:ba8cdae2652a 61 error_val = 0;
eil4nyqn 1:ba8cdae2652a 62 leds[0] = 0;
eil4nyqn 1:ba8cdae2652a 63 } else if(fep_temp==FEP_NO_RESPONSE) {
eil4nyqn 1:ba8cdae2652a 64 leds[0] = 1;
eil4nyqn 1:ba8cdae2652a 65 continue;
eil4nyqn 1:ba8cdae2652a 66 } else {
eil4nyqn 1:ba8cdae2652a 67 leds[0] = 1;
eil4nyqn 1:ba8cdae2652a 68 error_val++;
eil4nyqn 1:ba8cdae2652a 69 }
eil4nyqn 1:ba8cdae2652a 70 if(error_val < 4) {
tknara 9:8f9607783d2d 71 omni.computeXY(-1*con.getStick(2),con.getStick(3),con.getStick(0)/-2.0);
tknara 7:ef72ec7390c2 72 for(int i = 0; i < 4; i++) {
tknara 8:244c057d195c 73 pc.printf("%lf,",omni.getOutput(i));
tknara 7:ef72ec7390c2 74 wheels[i].setSpeed(omni.getOutput(i));
UCHITAKE 0:6c83a0871cc3 75 }
tknara 5:dee9310ec990 76 //昇降
tknara 9:8f9607783d2d 77 if(con.getButton1(0) == 1 && con.getButton1(1) == 0) {
tknara 9:8f9607783d2d 78 pwm = -0.9;
tknara 9:8f9607783d2d 79 } else if(con.getButton1(0) == 0 && con.getButton1(1) == 1) {
tknara 5:dee9310ec990 80 pwm = 0.9;
eil4nyqn 1:ba8cdae2652a 81 } else {
tknara 8:244c057d195c 82 pwm = 0.0;
eil4nyqn 1:ba8cdae2652a 83 }
tknara 7:ef72ec7390c2 84 lift[0].setSpeed(pwm);
tknara 6:259deb365510 85 //アーム攻撃(toggle)
tknara 9:8f9607783d2d 86 if((con.getButton2(1)==0)&&(airFlag1 == 0))
tknara 5:dee9310ec990 87 {
tknara 6:259deb365510 88 if(airStatus1==1) {
tknara 6:259deb365510 89 attack[0]=0;
tknara 6:259deb365510 90 attack[1]=1;
tknara 6:259deb365510 91 airFlag1=1;
tknara 6:259deb365510 92 airStatus1=0;
tknara 6:259deb365510 93 }else if(airStatus1==0) {
tknara 6:259deb365510 94 attack[0]=1;
tknara 6:259deb365510 95 attack[1]=0;
tknara 6:259deb365510 96 airFlag1=1;
tknara 6:259deb365510 97 airStatus1=1;
tknara 6:259deb365510 98 }
tknara 9:8f9607783d2d 99 }else if(con.getButton2(1)==1){
tknara 6:259deb365510 100 airFlag1=0;
tknara 5:dee9310ec990 101 attack[0]=0;
tknara 5:dee9310ec990 102 attack[1]=0;
tknara 5:dee9310ec990 103 }
tknara 6:259deb365510 104 //アーム角度(toggle)
tknara 9:8f9607783d2d 105 if((con.getButton2(3)==0)&&(airFlag2 == 0))
tknara 5:dee9310ec990 106 {
tknara 6:259deb365510 107 if(airStatus2==1) {
tknara 6:259deb365510 108 angle[0]=0;
tknara 6:259deb365510 109 angle[1]=1;
tknara 6:259deb365510 110 airFlag2=1;
tknara 6:259deb365510 111 airStatus2=0;
tknara 6:259deb365510 112 }else if(airStatus2==0) {
tknara 6:259deb365510 113 angle[0]=1;
tknara 6:259deb365510 114 angle[1]=0;
tknara 6:259deb365510 115 airFlag2=1;
tknara 6:259deb365510 116 airStatus2=1;
tknara 6:259deb365510 117 }
tknara 9:8f9607783d2d 118 }else if(con.getButton2(3)==1){
tknara 6:259deb365510 119 airFlag2=0;
tknara 5:dee9310ec990 120 angle[0]=0;
tknara 5:dee9310ec990 121 angle[1]=0;
tknara 5:dee9310ec990 122 }
tknara 6:259deb365510 123 #ifdef DEBUG
tknara 6:259deb365510 124 printf("Status of airFlags %d,%d\r\n",airStatus1,airStatus2);
tknara 6:259deb365510 125 #endif
tknara 5:dee9310ec990 126 } else if((fep_temp==FEP_NO_RESPONSE)&&(error_val > 4)){
tknara 5:dee9310ec990 127 AllActuatorStop();
eil4nyqn 1:ba8cdae2652a 128 } else {
tknara 5:dee9310ec990 129 AllActuatorStop();
UCHITAKE 0:6c83a0871cc3 130 }
UCHITAKE 0:6c83a0871cc3 131 }
UCHITAKE 0:6c83a0871cc3 132 }