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@15:d4ff132a616d, 2017-09-14 (annotated)

Committer:
tkNara
Date:
Thu Sep 14 18:22:29 2017 +0900
Revision:
15:d4ff132a616d
Parent:
14:1d9ae3128002
Child:
16:7eaf3343f45e
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tkNara 15:d4ff132a616d 1 #include "omni.h"
UCHITAKE 0:6c83a0871cc3 2 #include "mbed.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 15:d4ff132a616d 10 #define DEBUG
tknara 10:73148221684e 11
tkNara 15:d4ff132a616d 12 #define KP 0.0//1.5 //2.5
tkNara 15:d4ff132a616d 13 #define KI 0.0 //0s
tkNara 15:d4ff132a616d 14 #define KD 0.0 //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 15:d4ff132a616d 21 DigitalOut attack[2] = {air1_0,air1_1};
tkNara 15:d4ff132a616d 22 DigitalOut angle[2] = {air2_0,air2_1};
tknara 12:c7e91c4c2ffa 23 DigitalOut powerSw(powerSW);
tknara 10:73148221684e 24 R1370 R1370(R1370_TX,R1370_RX);
tkNara 15:d4ff132a616d 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 15:d4ff132a616d 42 omni.setWheelRadian(PI/4,3*PI/4,5*PI/4,7*PI/4);
tknara 9:8f9607783d2d 43 lift[0].braking = true;
WAT34 3:4cd170cdf049 44 }
tknara 10:73148221684e 45 void pidInit()
tknara 10:73148221684e 46 {
tknara 10:73148221684e 47 pid.reset();
tknara 10:73148221684e 48 pid.setInputLimits(-180.0,180.0);
tknara 10:73148221684e 49 pid.setOutputLimits(-1.0,1.0);
tknara 10:73148221684e 50 pid.setMode(1);
tknara 10:73148221684e 51 pid.setSetPoint(0.0);
tknara 10:73148221684e 52 pid.setBias(0.0);
tknara 10:73148221684e 53 }
tkNara 15:d4ff132a616d 54 void gyroInit()
tknara 5:dee9310ec990 55 {
tkNara 15:d4ff132a616d 56 R1370.update();
tkNara 15:d4ff132a616d 57 R1370.setZeroPoint(R1370.getAngle());
tknara 5:dee9310ec990 58 }
UCHITAKE 0:6c83a0871cc3 59 int main()
tkNara 15:d4ff132a616d 60 {
tknara 14:1d9ae3128002 61 bool airFlag1 = 0,airFlag2 = 0,airStatus1 = 0,airStatus2 = 0,pidflag = 0;
tknara 12:c7e91c4c2ffa 62 int error_val = 0,i = 0;
tkNara 15:d4ff132a616d 63 float nowAngle = 0;//,oldAngle = 0;
tknara 8:244c057d195c 64 init();
tknara 10:73148221684e 65 pidInit();
tkNara 15:d4ff132a616d 66 gyroInit();
tkNara 15:d4ff132a616d 67 while(1)
tknara 10:73148221684e 68 {
tkNara 15:d4ff132a616d 69 if(con.receiveState()==0)
tknara 10:73148221684e 70 {
tkNara 15:d4ff132a616d 71 if(R1370.update()==0)
tkNara 15:d4ff132a616d 72 {
tkNara 15:d4ff132a616d 73 if(con.getButton1(6)==0) {
tkNara 15:d4ff132a616d 74 powerSw = 0;
tkNara 15:d4ff132a616d 75 } else {
tkNara 15:d4ff132a616d 76 powerSw = 1;
tkNara 15:d4ff132a616d 77 }
tkNara 15:d4ff132a616d 78 error_val = 0;
tkNara 15:d4ff132a616d 79 nowAngle = R1370.getAngle();
tkNara 15:d4ff132a616d 80 if((con.getStick(0)==0)&&(pidflag==1)) {
tkNara 15:d4ff132a616d 81 pc.printf("zeroPointSet\n");
tkNara 15:d4ff132a616d 82 pidflag = 0;
tkNara 15:d4ff132a616d 83 R1370.setZeroPoint(nowAngle);
tkNara 15:d4ff132a616d 84 }else if(con.getStick(0)!=0) {
tkNara 15:d4ff132a616d 85 pidflag = 1;
tkNara 15:d4ff132a616d 86 omni.computeXY(-1*con.getStick(2),con.getStick(3),(-1*con.getStick(0))/3.0);
tkNara 15:d4ff132a616d 87 }else if((con.getStick(0)==0)&&(pidflag==0)) {
tkNara 15:d4ff132a616d 88 pid.setProcessValue(R1370.getDeviation(nowAngle));
tkNara 15:d4ff132a616d 89 omni.computeXY(-1*con.getStick(2),con.getStick(3),-1*pid.compute());
tkNara 15:d4ff132a616d 90 }
tkNara 15:d4ff132a616d 91 pc.printf("Now%lf:D%lf:set%lf:PID%lf ",R1370.getAngle(),R1370.getDeviation(nowAngle),R1370.getZeroPoint(),-1*pid.compute());
tkNara 15:d4ff132a616d 92 for(i = 0; i < 4; i++) {
tkNara 15:d4ff132a616d 93 pc.printf("%lf,",omni.getOutput(i));
tkNara 15:d4ff132a616d 94 wheels[i].setSpeed(omni.getOutput(i));
tkNara 15:d4ff132a616d 95 }
tkNara 15:d4ff132a616d 96 pc.printf("\n");
tkNara 15:d4ff132a616d 97 //昇降
tkNara 15:d4ff132a616d 98 lift[0].setSpeed(con.getStick(1));
tkNara 15:d4ff132a616d 99 //アーム攻撃(toggle)
tkNara 15:d4ff132a616d 100 pc.printf("%d\n",con.getButton2(1));
tkNara 15:d4ff132a616d 101 if((con.getButton2(1)==0)&&(airFlag1 == 0))
tkNara 15:d4ff132a616d 102 {
tkNara 15:d4ff132a616d 103 if(airStatus1==1) {
tkNara 15:d4ff132a616d 104 attack[0]=0;
tkNara 15:d4ff132a616d 105 attack[1]=1;
tkNara 15:d4ff132a616d 106 airFlag1=1;
tkNara 15:d4ff132a616d 107 airStatus1=0;
tkNara 15:d4ff132a616d 108 }else if(airStatus1==0) {
tkNara 15:d4ff132a616d 109 attack[0]=1;
tkNara 15:d4ff132a616d 110 attack[1]=0;
tkNara 15:d4ff132a616d 111 airFlag1=1;
tkNara 15:d4ff132a616d 112 airStatus1=1;
tkNara 15:d4ff132a616d 113 }
tkNara 15:d4ff132a616d 114 }else if(con.getButton2(1)==1){
tkNara 15:d4ff132a616d 115 airFlag1=0;
tkNara 15:d4ff132a616d 116 attack[0]=0;
tkNara 15:d4ff132a616d 117 attack[1]=0;
tkNara 15:d4ff132a616d 118 }
tkNara 15:d4ff132a616d 119 //アーム角度(toggle)
tkNara 15:d4ff132a616d 120 if((con.getButton2(3)==0)&&(airFlag2 == 0))
tkNara 15:d4ff132a616d 121 {
tkNara 15:d4ff132a616d 122 if(airStatus2==1) {
tkNara 15:d4ff132a616d 123 angle[0]=0;
tkNara 15:d4ff132a616d 124 angle[1]=1;
tkNara 15:d4ff132a616d 125 airFlag2=1;
tkNara 15:d4ff132a616d 126 airStatus2=0;
tkNara 15:d4ff132a616d 127 }else if(airStatus2==0) {
tkNara 15:d4ff132a616d 128 angle[0]=1;
tkNara 15:d4ff132a616d 129 angle[1]=0;
tkNara 15:d4ff132a616d 130 airFlag2=1;
tkNara 15:d4ff132a616d 131 airStatus2=1;
tkNara 15:d4ff132a616d 132 }
tkNara 15:d4ff132a616d 133 }else if(con.getButton2(3)==1){
tkNara 15:d4ff132a616d 134 airFlag2=0;
tkNara 15:d4ff132a616d 135 angle[0]=0;
tkNara 15:d4ff132a616d 136 angle[1]=0;
tkNara 15:d4ff132a616d 137 }
tknara 14:1d9ae3128002 138 }else {
tkNara 15:d4ff132a616d 139 //pc.printf("gyro_error\n");
tknara 5:dee9310ec990 140 }
tknara 12:c7e91c4c2ffa 141 }else {
tkNara 15:d4ff132a616d 142 //pc.printf("FEP_error\n");
tknara 12:c7e91c4c2ffa 143 error_val++;
tkNara 15:d4ff132a616d 144 if(error_val > 5) powerSw = 0;
tkNara 15:d4ff132a616d 145 else powerSw = 1;
UCHITAKE 0:6c83a0871cc3 146 }
UCHITAKE 0:6c83a0871cc3 147 }
UCHITAKE 0:6c83a0871cc3 148 }