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@11:a2e3d11f5750, 2017-09-04 (annotated)

Committer:
tknara
Date:
Mon Sep 04 04:29:54 2017 +0000
Revision:
11:a2e3d11f5750
Parent:
10:73148221684e
Child:
12:c7e91c4c2ffa
PID tuning 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"
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 10:73148221684e 12 #define KP 5
tknara 10:73148221684e 13 #define KI 0
tknara 10:73148221684e 14 #define KD 0
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 10:73148221684e 24 R1370 R1370(R1370_TX,R1370_RX);
tknara 10:73148221684e 25 PID pid(KP,KI,KD,RATE);
tknara 7:ef72ec7390c2 26 ikarashiMDC wheels[] {
tknara 8:244c057d195c 27 ikarashiMDC(&RS485control,0,0,SM,&RS485),
tknara 8:244c057d195c 28 ikarashiMDC(&RS485control,1,3,SM,&RS485),
tknara 8:244c057d195c 29 ikarashiMDC(&RS485control,1,0,SM,&RS485),
tknara 8:244c057d195c 30 ikarashiMDC(&RS485control,0,3,SM,&RS485)
tknara 7:ef72ec7390c2 31 };
tknara 7:ef72ec7390c2 32 ikarashiMDC lift[] {
tknara 9:8f9607783d2d 33 ikarashiMDC(&RS485control,1,1,SM,&RS485)
tknara 7:ef72ec7390c2 34 };
WAT34 3:4cd170cdf049 35 void init()
WAT34 3:4cd170cdf049 36 {
tknara 10:73148221684e 37 pc.printf("Hello\n");
tknara 8:244c057d195c 38 int i;
tknara 8:244c057d195c 39 for(i = 0;i < 4;i++) {
tknara 8:244c057d195c 40 wheels[i].braking = true;
tknara 8:244c057d195c 41 }
tknara 9:8f9607783d2d 42 lift[0].braking = true;
tknara 7:ef72ec7390c2 43 omni.setWheelRadian(PI/4,3*PI/4,5*PI/4,7*PI/4);
tknara 8:244c057d195c 44 for(i = 0;i<4;i++) {
tknara 10:73148221684e 45 //leds[i] = 0;
WAT34 3:4cd170cdf049 46 }
WAT34 3:4cd170cdf049 47 }
tknara 10:73148221684e 48 void pidInit()
tknara 10:73148221684e 49 {
tknara 10:73148221684e 50 pid.reset();
tknara 10:73148221684e 51 pid.setInputLimits(-180.0,180.0);
tknara 10:73148221684e 52 pid.setOutputLimits(-1.0,1.0);
tknara 10:73148221684e 53 pid.setMode(1);
tknara 10:73148221684e 54 pid.setSetPoint(0.0);
tknara 10:73148221684e 55 pid.setBias(0.0);
tknara 10:73148221684e 56 }
tknara 5:dee9310ec990 57 void AllActuatorStop()
tknara 5:dee9310ec990 58 {
tknara 5:dee9310ec990 59 #ifdef DEBUG
tknara 7:ef72ec7390c2 60 pc.printf("All actuators stop\n");
tknara 5:dee9310ec990 61 #endif
tknara 6:259deb365510 62 for(int i=0;i<1;i++)
tknara 6:259deb365510 63 {
tknara 6:259deb365510 64 attack[i]=0;
tknara 6:259deb365510 65 angle[i]=0;
tknara 6:259deb365510 66 }
tknara 5:dee9310ec990 67 }
UCHITAKE 0:6c83a0871cc3 68 int main()
tknara 6:259deb365510 69 {
tknara 6:259deb365510 70 bool airFlag1=0,airFlag2=0,airStatus1=0,airStatus2=0;
tknara 9:8f9607783d2d 71 int error_val = 0;
eil4nyqn 1:ba8cdae2652a 72 uint8_t fep_temp;
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 11:a2e3d11f5750 78 if(1)//con.receiveState()==0)
tknara 10:73148221684e 79 {
eil4nyqn 1:ba8cdae2652a 80 error_val = 0;
tknara 10:73148221684e 81 //leds[0] = 0;
tknara 11:a2e3d11f5750 82 if(R1370.reciveState() == 0)
tknara 10:73148221684e 83 {
tknara 10:73148221684e 84 //omni.computeXY(-1*con.getStick(2),con.getStick(3),con.getStick(0)/-2.0);
tknara 10:73148221684e 85 pid.setProcessValue(R1370.getAngle());
tknara 10:73148221684e 86 pc.printf("%lf %lf ",R1370.getAngle(),-1*pid.compute());
tknara 11:a2e3d11f5750 87 omni.computeXY(0,0,-1*pid.compute());
tknara 10:73148221684e 88 omni.computeXY(-1*con.getStick(2),con.getStick(3),-1*pid.compute());
tknara 10:73148221684e 89 for(int i = 0; i < 4; i++) {
tknara 10:73148221684e 90 pc.printf("%lf,",omni.getOutput(i));
tknara 10:73148221684e 91 wheels[i].setSpeed(omni.getOutput(i));
tknara 10:73148221684e 92 }
tknara 10:73148221684e 93 pc.printf("\n");
UCHITAKE 0:6c83a0871cc3 94 }
tknara 5:dee9310ec990 95 //昇降
tknara 9:8f9607783d2d 96 if(con.getButton1(0) == 1 && con.getButton1(1) == 0) {
tknara 9:8f9607783d2d 97 pwm = -0.9;
tknara 9:8f9607783d2d 98 } else if(con.getButton1(0) == 0 && con.getButton1(1) == 1) {
tknara 5:dee9310ec990 99 pwm = 0.9;
eil4nyqn 1:ba8cdae2652a 100 } else {
tknara 8:244c057d195c 101 pwm = 0.0;
eil4nyqn 1:ba8cdae2652a 102 }
tknara 7:ef72ec7390c2 103 lift[0].setSpeed(pwm);
tknara 6:259deb365510 104 //アーム攻撃(toggle)
tknara 9:8f9607783d2d 105 if((con.getButton2(1)==0)&&(airFlag1 == 0))
tknara 5:dee9310ec990 106 {
tknara 6:259deb365510 107 if(airStatus1==1) {
tknara 6:259deb365510 108 attack[0]=0;
tknara 6:259deb365510 109 attack[1]=1;
tknara 6:259deb365510 110 airFlag1=1;
tknara 6:259deb365510 111 airStatus1=0;
tknara 6:259deb365510 112 }else if(airStatus1==0) {
tknara 6:259deb365510 113 attack[0]=1;
tknara 6:259deb365510 114 attack[1]=0;
tknara 6:259deb365510 115 airFlag1=1;
tknara 6:259deb365510 116 airStatus1=1;
tknara 6:259deb365510 117 }
tknara 9:8f9607783d2d 118 }else if(con.getButton2(1)==1){
tknara 6:259deb365510 119 airFlag1=0;
tknara 5:dee9310ec990 120 attack[0]=0;
tknara 5:dee9310ec990 121 attack[1]=0;
tknara 5:dee9310ec990 122 }
tknara 6:259deb365510 123 //アーム角度(toggle)
tknara 9:8f9607783d2d 124 if((con.getButton2(3)==0)&&(airFlag2 == 0))
tknara 5:dee9310ec990 125 {
tknara 6:259deb365510 126 if(airStatus2==1) {
tknara 6:259deb365510 127 angle[0]=0;
tknara 6:259deb365510 128 angle[1]=1;
tknara 6:259deb365510 129 airFlag2=1;
tknara 6:259deb365510 130 airStatus2=0;
tknara 6:259deb365510 131 }else if(airStatus2==0) {
tknara 6:259deb365510 132 angle[0]=1;
tknara 6:259deb365510 133 angle[1]=0;
tknara 6:259deb365510 134 airFlag2=1;
tknara 6:259deb365510 135 airStatus2=1;
tknara 6:259deb365510 136 }
tknara 9:8f9607783d2d 137 }else if(con.getButton2(3)==1){
tknara 6:259deb365510 138 airFlag2=0;
tknara 5:dee9310ec990 139 angle[0]=0;
tknara 5:dee9310ec990 140 angle[1]=0;
tknara 5:dee9310ec990 141 }
UCHITAKE 0:6c83a0871cc3 142 }
UCHITAKE 0:6c83a0871cc3 143 }
UCHITAKE 0:6c83a0871cc3 144 }