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

Committer:
tknara
Date:
Mon Sep 04 05:33:42 2017 +0000
Revision:
12:c7e91c4c2ffa
Parent:
11:a2e3d11f5750
Child:
14:1d9ae3128002
NO PID Tuning

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 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 6:259deb365510 71 bool airFlag1=0,airFlag2=0,airStatus1=0,airStatus2=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 pid.setProcessValue(R1370.getAngle());
tknara 12:c7e91c4c2ffa 87 pc.printf("%lf %lf ",R1370.getAngle(),-1*pid.compute());
tknara 12:c7e91c4c2ffa 88 omni.computeXY(0,0,-1*pid.compute());
tknara 12:c7e91c4c2ffa 89 omni.computeXY(-1*con.getStick(2),con.getStick(3),-1*pid.compute());
tknara 12:c7e91c4c2ffa 90 for(int i = 0; i < 4; i++) {
tknara 12:c7e91c4c2ffa 91 pc.printf("%lf,",omni.getOutput(i));
tknara 12:c7e91c4c2ffa 92 wheels[i].setSpeed(omni.getOutput(i));
UCHITAKE 0:6c83a0871cc3 93 }
tknara 12:c7e91c4c2ffa 94 pc.printf("\n");
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 }
tknara 12:c7e91c4c2ffa 142 }else {
tknara 12:c7e91c4c2ffa 143 error_val++;
tknara 12:c7e91c4c2ffa 144 if(error_val > 10) powerSw = 0;
UCHITAKE 0:6c83a0871cc3 145 }
UCHITAKE 0:6c83a0871cc3 146 }
UCHITAKE 0:6c83a0871cc3 147 }