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

Committer:
tkNara
Date:
Fri Sep 15 10:14:41 2017 +0900
Revision:
16:7eaf3343f45e
Parent:
15:d4ff132a616d 
update FEP baud 115200

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 16:7eaf3343f45e 63 float nowAngle = 0,rote = 0.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 16:7eaf3343f45e 78 for(i=0;i<6;i++){
tkNara 16:7eaf3343f45e 79 pc.printf("%d,%d\n",i,con.getButton2(i));
tkNara 16:7eaf3343f45e 80 }
tkNara 15:d4ff132a616d 81 error_val = 0;
tkNara 15:d4ff132a616d 82 nowAngle = R1370.getAngle();
tkNara 16:7eaf3343f45e 83 /*if((con.getStick(0)==0)&&(pidflag==1)) {
tkNara 15:d4ff132a616d 84 pc.printf("zeroPointSet\n");
tkNara 15:d4ff132a616d 85 pidflag = 0;
tkNara 15:d4ff132a616d 86 R1370.setZeroPoint(nowAngle);
tkNara 15:d4ff132a616d 87 }else if(con.getStick(0)!=0) {
tkNara 15:d4ff132a616d 88 pidflag = 1;
tkNara 16:7eaf3343f45e 89 if((con.getButton2(0)==0)&&(con.getButton2(2)==1)){
tkNara 16:7eaf3343f45e 90 rote = 0.15;
tkNara 16:7eaf3343f45e 91 }else if((con.getButton2(0)==1)&&(con.getButton2(2)==0)){
tkNara 16:7eaf3343f45e 92 rote = -0.15;
tkNara 16:7eaf3343f45e 93 }else {
tkNara 16:7eaf3343f45e 94 rote = 0.0;
tkNara 16:7eaf3343f45e 95 }
tkNara 16:7eaf3343f45e 96 omni.computeXY(-1*con.getStick(2),con.getStick(3),rote);//(-1*con.getStick(0))/3.0);
tkNara 15:d4ff132a616d 97 }else if((con.getStick(0)==0)&&(pidflag==0)) {
tkNara 15:d4ff132a616d 98 pid.setProcessValue(R1370.getDeviation(nowAngle));
tkNara 15:d4ff132a616d 99 omni.computeXY(-1*con.getStick(2),con.getStick(3),-1*pid.compute());
tkNara 16:7eaf3343f45e 100 }*/
tkNara 16:7eaf3343f45e 101 if((con.getButton2(0)==0)&&(con.getButton2(2)==1)){
tkNara 16:7eaf3343f45e 102 rote = 0.15;
tkNara 16:7eaf3343f45e 103 }else if((con.getButton2(0)==1)&&(con.getButton2(2)==0)){
tkNara 16:7eaf3343f45e 104 rote = -0.15;
tkNara 16:7eaf3343f45e 105 }else {
tkNara 16:7eaf3343f45e 106 rote = 0.0;
tkNara 15:d4ff132a616d 107 }
tkNara 16:7eaf3343f45e 108 omni.computeXY(-1*con.getStick(2),con.getStick(3),rote);
tkNara 15:d4ff132a616d 109 pc.printf("Now%lf:D%lf:set%lf:PID%lf ",R1370.getAngle(),R1370.getDeviation(nowAngle),R1370.getZeroPoint(),-1*pid.compute());
tkNara 15:d4ff132a616d 110 for(i = 0; i < 4; i++) {
tkNara 15:d4ff132a616d 111 pc.printf("%lf,",omni.getOutput(i));
tkNara 15:d4ff132a616d 112 wheels[i].setSpeed(omni.getOutput(i));
tkNara 15:d4ff132a616d 113 }
tkNara 15:d4ff132a616d 114 pc.printf("\n");
tkNara 15:d4ff132a616d 115 //昇降
tkNara 15:d4ff132a616d 116 lift[0].setSpeed(con.getStick(1));
tkNara 15:d4ff132a616d 117 //アーム攻撃(toggle)
tkNara 15:d4ff132a616d 118 pc.printf("%d\n",con.getButton2(1));
tkNara 15:d4ff132a616d 119 if((con.getButton2(1)==0)&&(airFlag1 == 0))
tkNara 15:d4ff132a616d 120 {
tkNara 15:d4ff132a616d 121 if(airStatus1==1) {
tkNara 15:d4ff132a616d 122 attack[0]=0;
tkNara 15:d4ff132a616d 123 attack[1]=1;
tkNara 15:d4ff132a616d 124 airFlag1=1;
tkNara 15:d4ff132a616d 125 airStatus1=0;
tkNara 15:d4ff132a616d 126 }else if(airStatus1==0) {
tkNara 15:d4ff132a616d 127 attack[0]=1;
tkNara 15:d4ff132a616d 128 attack[1]=0;
tkNara 15:d4ff132a616d 129 airFlag1=1;
tkNara 15:d4ff132a616d 130 airStatus1=1;
tkNara 15:d4ff132a616d 131 }
tkNara 15:d4ff132a616d 132 }else if(con.getButton2(1)==1){
tkNara 15:d4ff132a616d 133 airFlag1=0;
tkNara 15:d4ff132a616d 134 attack[0]=0;
tkNara 15:d4ff132a616d 135 attack[1]=0;
tkNara 15:d4ff132a616d 136 }
tkNara 15:d4ff132a616d 137 //アーム角度(toggle)
tkNara 15:d4ff132a616d 138 if((con.getButton2(3)==0)&&(airFlag2 == 0))
tkNara 15:d4ff132a616d 139 {
tkNara 15:d4ff132a616d 140 if(airStatus2==1) {
tkNara 15:d4ff132a616d 141 angle[0]=0;
tkNara 15:d4ff132a616d 142 angle[1]=1;
tkNara 15:d4ff132a616d 143 airFlag2=1;
tkNara 15:d4ff132a616d 144 airStatus2=0;
tkNara 15:d4ff132a616d 145 }else if(airStatus2==0) {
tkNara 15:d4ff132a616d 146 angle[0]=1;
tkNara 15:d4ff132a616d 147 angle[1]=0;
tkNara 15:d4ff132a616d 148 airFlag2=1;
tkNara 15:d4ff132a616d 149 airStatus2=1;
tkNara 15:d4ff132a616d 150 }
tkNara 15:d4ff132a616d 151 }else if(con.getButton2(3)==1){
tkNara 15:d4ff132a616d 152 airFlag2=0;
tkNara 15:d4ff132a616d 153 angle[0]=0;
tkNara 15:d4ff132a616d 154 angle[1]=0;
tkNara 15:d4ff132a616d 155 }
tknara 14:1d9ae3128002 156 }else {
tkNara 15:d4ff132a616d 157 //pc.printf("gyro_error\n");
tknara 5:dee9310ec990 158 }
tknara 12:c7e91c4c2ffa 159 }else {
tkNara 15:d4ff132a616d 160 //pc.printf("FEP_error\n");
tknara 12:c7e91c4c2ffa 161 error_val++;
tkNara 15:d4ff132a616d 162 if(error_val > 5) powerSw = 0;
tkNara 15:d4ff132a616d 163 else powerSw = 1;
UCHITAKE 0:6c83a0871cc3 164 }
UCHITAKE 0:6c83a0871cc3 165 }
UCHITAKE 0:6c83a0871cc3 166 }