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@14:1d9ae3128002, 2017-09-05 (annotated)

Committer:
tknara
Date:
Tue Sep 05 00:41:30 2017 +0000
Revision:
14:1d9ae3128002
Parent:
12:c7e91c4c2ffa
Child:
15:d4ff132a616d
Rotation activate;

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"
tknara 10:73148221684e 7 #include "PID.h"
tknara 10:73148221684e 8 #include "R1370.h"
UCHITAKE 0:6c83a0871cc3 9
tknara 5:dee9310ec990 10 #define DEBUG
tknara 10:73148221684e 11
tknara 14:1d9ae3128002 12 #define KP 2.5
tknara 10:73148221684e 13 #define KI 0
tknara 14:1d9ae3128002 14 #define KD 0.05
tknara 10:73148221684e 15 #define RATE 0.01
tknara 7:ef72ec7390c2 16 Omni omni(4);
tknara 9:8f9607783d2d 17 Serial RS485(RS485_TX,RS485_RX,38400);
tknara 9:8f9607783d2d 18 Serial pc(USBTX,USBRX,115200);
tknara 9:8f9607783d2d 19 Controller con(FEP_TX,FEP_RX,200);
tknara 10:73148221684e 20 DigitalOut RS485control(PA_4);
tknara 10:73148221684e 21 //DigitalOut leds[4] = {PC_13,PC_14,PC_15,PA_0};
tknara 5:dee9310ec990 22 DigitalOut attack[2] = {PA_10,PC_4};
tknara 5:dee9310ec990 23 DigitalOut angle[2] = {PB_3,PB_5};
tknara 12:c7e91c4c2ffa 24 DigitalOut powerSw(powerSW);
tknara 10:73148221684e 25 R1370 R1370(R1370_TX,R1370_RX);
tknara 10:73148221684e 26 PID pid(KP,KI,KD,RATE);
tknara 7:ef72ec7390c2 27 ikarashiMDC wheels[] {
tknara 8:244c057d195c 28 ikarashiMDC(&RS485control,0,0,SM,&RS485),
tknara 8:244c057d195c 29 ikarashiMDC(&RS485control,1,3,SM,&RS485),
tknara 8:244c057d195c 30 ikarashiMDC(&RS485control,1,0,SM,&RS485),
tknara 8:244c057d195c 31 ikarashiMDC(&RS485control,0,3,SM,&RS485)
tknara 7:ef72ec7390c2 32 };
tknara 7:ef72ec7390c2 33 ikarashiMDC lift[] {
tknara 9:8f9607783d2d 34 ikarashiMDC(&RS485control,1,1,SM,&RS485)
tknara 7:ef72ec7390c2 35 };
WAT34 3:4cd170cdf049 36 void init()
WAT34 3:4cd170cdf049 37 {
tknara 10:73148221684e 38 pc.printf("Hello\n");
tknara 8:244c057d195c 39 int i;
tknara 8:244c057d195c 40 for(i = 0;i < 4;i++) {
tknara 8:244c057d195c 41 wheels[i].braking = true;
tknara 8:244c057d195c 42 }
tknara 9:8f9607783d2d 43 lift[0].braking = true;
tknara 7:ef72ec7390c2 44 omni.setWheelRadian(PI/4,3*PI/4,5*PI/4,7*PI/4);
tknara 8:244c057d195c 45 for(i = 0;i<4;i++) {
tknara 10:73148221684e 46 //leds[i] = 0;
WAT34 3:4cd170cdf049 47 }
WAT34 3:4cd170cdf049 48 }
tknara 10:73148221684e 49 void pidInit()
tknara 10:73148221684e 50 {
tknara 10:73148221684e 51 pid.reset();
tknara 10:73148221684e 52 pid.setInputLimits(-180.0,180.0);
tknara 10:73148221684e 53 pid.setOutputLimits(-1.0,1.0);
tknara 10:73148221684e 54 pid.setMode(1);
tknara 10:73148221684e 55 pid.setSetPoint(0.0);
tknara 10:73148221684e 56 pid.setBias(0.0);
tknara 10:73148221684e 57 }
tknara 5:dee9310ec990 58 void AllActuatorStop()
tknara 5:dee9310ec990 59 {
tknara 5:dee9310ec990 60 #ifdef DEBUG
tknara 7:ef72ec7390c2 61 pc.printf("All actuators stop\n");
tknara 5:dee9310ec990 62 #endif
tknara 6:259deb365510 63 for(int i=0;i<1;i++)
tknara 6:259deb365510 64 {
tknara 6:259deb365510 65 attack[i]=0;
tknara 6:259deb365510 66 angle[i]=0;
tknara 6:259deb365510 67 }
tknara 5:dee9310ec990 68 }
UCHITAKE 0:6c83a0871cc3 69 int main()
tknara 6:259deb365510 70 {
tknara 14:1d9ae3128002 71 bool airFlag1 = 0,airFlag2 = 0,airStatus1 = 0,airStatus2 = 0,pidflag = 0;
tknara 12:c7e91c4c2ffa 72 int error_val = 0,i = 0;
tknara 5:dee9310ec990 73 double pwm = 0.0;
tknara 8:244c057d195c 74 init();
tknara 10:73148221684e 75 pidInit();
tknara 10:73148221684e 76 while(1)
tknara 10:73148221684e 77 {
tknara 12:c7e91c4c2ffa 78 if((con.receiveState()==0)&&(R1370.update()==0))
tknara 10:73148221684e 79 {
tknara 12:c7e91c4c2ffa 80 if(con.getButton1(6)==0) {
tknara 12:c7e91c4c2ffa 81 powerSw = 0;
tknara 12:c7e91c4c2ffa 82 } else {
tknara 12:c7e91c4c2ffa 83 powerSw = 1;
tknara 12:c7e91c4c2ffa 84 }
eil4nyqn 1:ba8cdae2652a 85 error_val = 0;
tknara 12:c7e91c4c2ffa 86 pc.printf("%lf %lf ",R1370.getAngle(),-1*pid.compute());
tknara 14:1d9ae3128002 87 if((con.getStick(0)==0)&&(pidflag==1)) {
tknara 14:1d9ae3128002 88 pidflag = 0;
tknara 14:1d9ae3128002 89 pid.setSetPoint(R1370.getAngle());;
tknara 14:1d9ae3128002 90 }else {
tknara 14:1d9ae3128002 91 pidflag = 1;
tknara 14:1d9ae3128002 92 omni.computeXY(-1*con.getStick(2),con.getStick(3),con.getStick(0));
tknara 14:1d9ae3128002 93 }
tknara 14:1d9ae3128002 94 pid.setProcessValue(R1370.getAngle());
tknara 12:c7e91c4c2ffa 95 for(int i = 0; i < 4; i++) {
tknara 12:c7e91c4c2ffa 96 pc.printf("%lf,",omni.getOutput(i));
tknara 12:c7e91c4c2ffa 97 wheels[i].setSpeed(omni.getOutput(i));
UCHITAKE 0:6c83a0871cc3 98 }
tknara 12:c7e91c4c2ffa 99 pc.printf("\n");
tknara 5:dee9310ec990 100 //昇降
tknara 9:8f9607783d2d 101 if(con.getButton1(0) == 1 && con.getButton1(1) == 0) {
tknara 9:8f9607783d2d 102 pwm = -0.9;
tknara 9:8f9607783d2d 103 } else if(con.getButton1(0) == 0 && con.getButton1(1) == 1) {
tknara 5:dee9310ec990 104 pwm = 0.9;
eil4nyqn 1:ba8cdae2652a 105 } else {
tknara 8:244c057d195c 106 pwm = 0.0;
eil4nyqn 1:ba8cdae2652a 107 }
tknara 7:ef72ec7390c2 108 lift[0].setSpeed(pwm);
tknara 6:259deb365510 109 //アーム攻撃(toggle)
tknara 9:8f9607783d2d 110 if((con.getButton2(1)==0)&&(airFlag1 == 0))
tknara 5:dee9310ec990 111 {
tknara 6:259deb365510 112 if(airStatus1==1) {
tknara 6:259deb365510 113 attack[0]=0;
tknara 6:259deb365510 114 attack[1]=1;
tknara 6:259deb365510 115 airFlag1=1;
tknara 6:259deb365510 116 airStatus1=0;
tknara 6:259deb365510 117 }else if(airStatus1==0) {
tknara 6:259deb365510 118 attack[0]=1;
tknara 6:259deb365510 119 attack[1]=0;
tknara 6:259deb365510 120 airFlag1=1;
tknara 6:259deb365510 121 airStatus1=1;
tknara 6:259deb365510 122 }
tknara 9:8f9607783d2d 123 }else if(con.getButton2(1)==1){
tknara 6:259deb365510 124 airFlag1=0;
tknara 5:dee9310ec990 125 attack[0]=0;
tknara 5:dee9310ec990 126 attack[1]=0;
tknara 5:dee9310ec990 127 }
tknara 6:259deb365510 128 //アーム角度(toggle)
tknara 9:8f9607783d2d 129 if((con.getButton2(3)==0)&&(airFlag2 == 0))
tknara 5:dee9310ec990 130 {
tknara 6:259deb365510 131 if(airStatus2==1) {
tknara 6:259deb365510 132 angle[0]=0;
tknara 6:259deb365510 133 angle[1]=1;
tknara 6:259deb365510 134 airFlag2=1;
tknara 6:259deb365510 135 airStatus2=0;
tknara 6:259deb365510 136 }else if(airStatus2==0) {
tknara 6:259deb365510 137 angle[0]=1;
tknara 6:259deb365510 138 angle[1]=0;
tknara 6:259deb365510 139 airFlag2=1;
tknara 6:259deb365510 140 airStatus2=1;
tknara 6:259deb365510 141 }
tknara 9:8f9607783d2d 142 }else if(con.getButton2(3)==1){
tknara 6:259deb365510 143 airFlag2=0;
tknara 5:dee9310ec990 144 angle[0]=0;
tknara 5:dee9310ec990 145 angle[1]=0;
tknara 5:dee9310ec990 146 }
tknara 12:c7e91c4c2ffa 147 }else {
tknara 12:c7e91c4c2ffa 148 error_val++;
tknara 12:c7e91c4c2ffa 149 if(error_val > 10) powerSw = 0;
UCHITAKE 0:6c83a0871cc3 150 }
tknara 14:1d9ae3128002 151 wait(RATE);
UCHITAKE 0:6c83a0871cc3 152 }
UCHITAKE 0:6c83a0871cc3 153 }