NagaokaRoboticsClub_mbedTeam / Mbed OS NHK2019_usiro_v6

NHK2019_Team_B_Automatic_machine_usirogawa

Dependencies:   SerialMultiByte QEI omni_wheel PID R1370MeasuringWheel IRsensor ikarashiMDC_2byte_ver Eigen

main.cpp@0:3ad208cbea5f, 2019-09-13 (annotated)

Committer:
skouki
Date:
Fri Sep 13 02:19:03 2019 +0000
Revision:
0:3ad208cbea5f
Child:
1:64871263444f
2019/09/13ver

Who changed what in which revision?

UserRevisionLine numberNew contents of line
skouki 0:3ad208cbea5f 1 #include"mbed.h"
skouki 0:3ad208cbea5f 2 #include"measuring_wheel.h"
skouki 0:3ad208cbea5f 3 #include"ikarashiMDC.h"
skouki 0:3ad208cbea5f 4 #include"position_controller.h"
skouki 0:3ad208cbea5f 5 #include"omni_wheel.h"
skouki 0:3ad208cbea5f 6 #include"pin_config.h"
skouki 0:3ad208cbea5f 7 #include"SerialMultiByte.h"
skouki 0:3ad208cbea5f 8 #include"PID.h"
skouki 0:3ad208cbea5f 9 #include"IRsensor.h"
skouki 0:3ad208cbea5f 10
skouki 0:3ad208cbea5f 11 #define YPOINT 4000
skouki 0:3ad208cbea5f 12 #define GAP 0
skouki 0:3ad208cbea5f 13
skouki 0:3ad208cbea5f 14 Serial serial(MDCTX,MDCRX,115200);
skouki 0:3ad208cbea5f 15 ikarashiMDC motor[]={
skouki 0:3ad208cbea5f 16 ikarashiMDC(1,0,SM,&serial),
skouki 0:3ad208cbea5f 17 ikarashiMDC(1,1,SM,&serial),
skouki 0:3ad208cbea5f 18 ikarashiMDC(1,2,SM,&serial),
skouki 0:3ad208cbea5f 19 ikarashiMDC(1,3,SM,&serial)
skouki 0:3ad208cbea5f 20 };
skouki 0:3ad208cbea5f 21 PositionController position_control_1(500,500,0.1,0.01,0.8);
skouki 0:3ad208cbea5f 22
skouki 0:3ad208cbea5f 23 OmniWheel omni(4);
skouki 0:3ad208cbea5f 24 SerialMultiByte s(SERIALTX,SERIALRX);
skouki 0:3ad208cbea5f 25 MeasuringWheel m(QEI1_1,QEI1_2,QEI4_1,QEI4_2,QEI3_1,QEI3_2,R1370TX,R1370RX);
skouki 0:3ad208cbea5f 26 PID pid_spin(0,0,0,0.001);
skouki 0:3ad208cbea5f 27 PID pid_x(0,0,0,0.001);
skouki 0:3ad208cbea5f 28 PID pid_y(0,0,0,0.001);
skouki 0:3ad208cbea5f 29 Serial pc(USBTX,USBRX,115200);
skouki 0:3ad208cbea5f 30
skouki 0:3ad208cbea5f 31 DigitalIn debug_button(USER_BUTTON);
skouki 0:3ad208cbea5f 32 DigitalOut debug_led_0(LED_0);
skouki 0:3ad208cbea5f 33 DigitalOut debug_led_1(LED_2);
skouki 0:3ad208cbea5f 34 DigitalOut debug_led_2(LED_1);
skouki 0:3ad208cbea5f 35
skouki 0:3ad208cbea5f 36 IRsensor IR0(IR_0);
skouki 0:3ad208cbea5f 37
skouki 0:3ad208cbea5f 38 int mode;
skouki 0:3ad208cbea5f 39 int instruction_mode;
skouki 0:3ad208cbea5f 40 double omni_power[4];
skouki 0:3ad208cbea5f 41 double X_power,Y_power;
skouki 0:3ad208cbea5f 42 double spin_power;
skouki 0:3ad208cbea5f 43 float y_point = YPOINT;
skouki 0:3ad208cbea5f 44 int X_,Y_;
skouki 0:3ad208cbea5f 45 double dai_x,dai_low_y;
skouki 0:3ad208cbea5f 46 int gap = GAP;
skouki 0:3ad208cbea5f 47
skouki 0:3ad208cbea5f 48 void set_up()
skouki 0:3ad208cbea5f 49 {
skouki 0:3ad208cbea5f 50 float theta = PIII / 4;
skouki 0:3ad208cbea5f 51 omni.wheel[0].setRadian(PIII - theta);
skouki 0:3ad208cbea5f 52 omni.wheel[1].setRadian(theta);
skouki 0:3ad208cbea5f 53 omni.wheel[2].setRadian(-theta);
skouki 0:3ad208cbea5f 54 omni.wheel[3].setRadian(PIII + theta);
skouki 0:3ad208cbea5f 55
skouki 0:3ad208cbea5f 56 s.setHeaders('A','Z');
skouki 0:3ad208cbea5f 57 s.startReceive(5);
skouki 0:3ad208cbea5f 58
skouki 0:3ad208cbea5f 59 IR0.startAveraging(5);
skouki 0:3ad208cbea5f 60
skouki 0:3ad208cbea5f 61 }
skouki 0:3ad208cbea5f 62
skouki 0:3ad208cbea5f 63 void mode1()
skouki 0:3ad208cbea5f 64 {
skouki 0:3ad208cbea5f 65 pid_x.setProcessValue(m.getOutX());
skouki 0:3ad208cbea5f 66 X_power += pid_x.compute();
skouki 0:3ad208cbea5f 67
skouki 0:3ad208cbea5f 68 position_control_1.compute(0.0,m.getOutY());
skouki 0:3ad208cbea5f 69 Y_power += position_control_1.getVelocityY();
skouki 0:3ad208cbea5f 70
skouki 0:3ad208cbea5f 71 pid_y.setProcessValue(m.getOutY() - Y_);
skouki 0:3ad208cbea5f 72 Y_power += pid_y.compute();
skouki 0:3ad208cbea5f 73
skouki 0:3ad208cbea5f 74 pid_spin.setProcessValue(m.getjyroAngle());
skouki 0:3ad208cbea5f 75 spin_power = pid_spin.compute();
skouki 0:3ad208cbea5f 76
skouki 0:3ad208cbea5f 77 }
skouki 0:3ad208cbea5f 78
skouki 0:3ad208cbea5f 79 void mode2()
skouki 0:3ad208cbea5f 80 {
skouki 0:3ad208cbea5f 81 X_power -= 0.3;
skouki 0:3ad208cbea5f 82
skouki 0:3ad208cbea5f 83 pid_x.setProcessValue(m.getOutX() - X_);
skouki 0:3ad208cbea5f 84 X_power += pid_x.compute();
skouki 0:3ad208cbea5f 85
skouki 0:3ad208cbea5f 86 pid_y.setProcessValue(m.getOutY());
skouki 0:3ad208cbea5f 87 Y_power += pid_y.compute();
skouki 0:3ad208cbea5f 88
skouki 0:3ad208cbea5f 89 pid_spin.setProcessValue(m.getjyroAngle());
skouki 0:3ad208cbea5f 90 spin_power = pid_spin.compute();
skouki 0:3ad208cbea5f 91
skouki 0:3ad208cbea5f 92 }
skouki 0:3ad208cbea5f 93
skouki 0:3ad208cbea5f 94 void mode3()
skouki 0:3ad208cbea5f 95 {
skouki 0:3ad208cbea5f 96 pid_x.setProcessValue(m.getOutX());
skouki 0:3ad208cbea5f 97 X_power += pid_x.compute();
skouki 0:3ad208cbea5f 98
skouki 0:3ad208cbea5f 99 Y_power -= 0.3;
skouki 0:3ad208cbea5f 100
skouki 0:3ad208cbea5f 101 pid_spin.setProcessValue(m.getjyroAngle());
skouki 0:3ad208cbea5f 102 spin_power = pid_spin.compute();
skouki 0:3ad208cbea5f 103
skouki 0:3ad208cbea5f 104 }
skouki 0:3ad208cbea5f 105
skouki 0:3ad208cbea5f 106 void mode4()
skouki 0:3ad208cbea5f 107 {
skouki 0:3ad208cbea5f 108 pid_x.setProcessValue(m.getOutX());
skouki 0:3ad208cbea5f 109 X_power += pid_x.compute();
skouki 0:3ad208cbea5f 110
skouki 0:3ad208cbea5f 111 pid_y.setProcessValue(m.getOutY());
skouki 0:3ad208cbea5f 112 Y_power += pid_y.compute();
skouki 0:3ad208cbea5f 113
skouki 0:3ad208cbea5f 114 pid_spin.setProcessValue(m.getjyroAngle());
skouki 0:3ad208cbea5f 115 spin_power = pid_spin.compute();
skouki 0:3ad208cbea5f 116
skouki 0:3ad208cbea5f 117 }
skouki 0:3ad208cbea5f 118
skouki 0:3ad208cbea5f 119 void to_main()
skouki 0:3ad208cbea5f 120 {
skouki 0:3ad208cbea5f 121 unsigned char data[1];
skouki 0:3ad208cbea5f 122 unsigned char getdata[5];
skouki 0:3ad208cbea5f 123 data[0] = mode;
skouki 0:3ad208cbea5f 124 s.sendData(data,1);
skouki 0:3ad208cbea5f 125 s.getData(getdata);
skouki 0:3ad208cbea5f 126 instruction_mode=getdata[0];
skouki 0:3ad208cbea5f 127
skouki 0:3ad208cbea5f 128 if(getdata[2]>=(1<<7)){
skouki 0:3ad208cbea5f 129 getdata[2]-=(1<<7);
skouki 0:3ad208cbea5f 130 X_=-1*(getdata[1]+(getdata[2]<<8));
skouki 0:3ad208cbea5f 131 }
skouki 0:3ad208cbea5f 132 else X_= getdata[1]+(getdata[2]<<8);
skouki 0:3ad208cbea5f 133
skouki 0:3ad208cbea5f 134 if(getdata[4]>=(1<<7)){
skouki 0:3ad208cbea5f 135 getdata[4]-=(1<<7);
skouki 0:3ad208cbea5f 136 Y_=-1*(getdata[3]+(getdata[4]<<8));
skouki 0:3ad208cbea5f 137 }
skouki 0:3ad208cbea5f 138 else Y_= getdata[3]+(getdata[4]<<8);
skouki 0:3ad208cbea5f 139 if(instruction_mode!=0)debug_led_1 = !debug_led_1;
skouki 0:3ad208cbea5f 140 }
skouki 0:3ad208cbea5f 141 int main()
skouki 0:3ad208cbea5f 142 {
skouki 0:3ad208cbea5f 143 set_up();
skouki 0:3ad208cbea5f 144 while(true){
skouki 0:3ad208cbea5f 145 debug_led_0 = !debug_led_0;
skouki 0:3ad208cbea5f 146 X_power = 0.0;
skouki 0:3ad208cbea5f 147 Y_power = 0.0;
skouki 0:3ad208cbea5f 148 spin_power = 0.0;
skouki 0:3ad208cbea5f 149
skouki 0:3ad208cbea5f 150 to_main();
skouki 0:3ad208cbea5f 151
skouki 0:3ad208cbea5f 152 if(instruction_mode == 1&&mode == 0){
skouki 0:3ad208cbea5f 153
skouki 0:3ad208cbea5f 154 pid_x.reset();
skouki 0:3ad208cbea5f 155 pid_x.setTunings(3.0,1.0,0.000001);
skouki 0:3ad208cbea5f 156 pid_x.setInputLimits(-1000.0,1000.0);
skouki 0:3ad208cbea5f 157 pid_x.setOutputLimits(-1.0,1.0);
skouki 0:3ad208cbea5f 158 pid_x.setBias(0);
skouki 0:3ad208cbea5f 159 pid_x.setMode(1);
skouki 0:3ad208cbea5f 160 pid_x.setSetPoint(0.0);
skouki 0:3ad208cbea5f 161
skouki 0:3ad208cbea5f 162 position_control_1.targetXY(1,int(y_point));
skouki 0:3ad208cbea5f 163
skouki 0:3ad208cbea5f 164 pid_y.reset();
skouki 0:3ad208cbea5f 165 pid_y.setTunings(1.0,1.0,0.000001);
skouki 0:3ad208cbea5f 166 pid_y.setInputLimits(-1000.0,1000.0);
skouki 0:3ad208cbea5f 167 pid_y.setOutputLimits(-1.0,1.0);
skouki 0:3ad208cbea5f 168 pid_y.setBias(0);
skouki 0:3ad208cbea5f 169 pid_y.setMode(1);
skouki 0:3ad208cbea5f 170 pid_y.setSetPoint(0.0);
skouki 0:3ad208cbea5f 171
skouki 0:3ad208cbea5f 172 pid_spin.reset();
skouki 0:3ad208cbea5f 173 pid_spin.setTunings(5.0,1.0,0.000001);
skouki 0:3ad208cbea5f 174 pid_spin.setInputLimits(-180.0,180.0);
skouki 0:3ad208cbea5f 175 pid_spin.setOutputLimits(-0.5,0.5);
skouki 0:3ad208cbea5f 176 pid_spin.setBias(0);
skouki 0:3ad208cbea5f 177 pid_spin.setMode(1);
skouki 0:3ad208cbea5f 178 pid_spin.setSetPoint(0.0);
skouki 0:3ad208cbea5f 179
skouki 0:3ad208cbea5f 180 mode = 1;
skouki 0:3ad208cbea5f 181 }
skouki 0:3ad208cbea5f 182
skouki 0:3ad208cbea5f 183 if((m.getOutY() >= (y_point - 50)) && mode == 1){
skouki 0:3ad208cbea5f 184
skouki 0:3ad208cbea5f 185 pid_x.reset();
skouki 0:3ad208cbea5f 186 pid_x.setTunings(1.0,1.0,0.000001);
skouki 0:3ad208cbea5f 187 pid_x.setInputLimits(-1000.0,1000.0);
skouki 0:3ad208cbea5f 188 pid_x.setOutputLimits(-1.0,1.0);
skouki 0:3ad208cbea5f 189 pid_x.setBias(0);
skouki 0:3ad208cbea5f 190 pid_x.setMode(1);
skouki 0:3ad208cbea5f 191 pid_x.setSetPoint(0.0);
skouki 0:3ad208cbea5f 192
skouki 0:3ad208cbea5f 193 pid_y.reset();
skouki 0:3ad208cbea5f 194 pid_y.setTunings(1.0,1.0,0.000001);
skouki 0:3ad208cbea5f 195 pid_y.setInputLimits(y_point - 1000.0 , y_point + 1000.0);
skouki 0:3ad208cbea5f 196 pid_y.setOutputLimits(-1.0,1.0);
skouki 0:3ad208cbea5f 197 pid_y.setBias(0);
skouki 0:3ad208cbea5f 198 pid_y.setMode(1);
skouki 0:3ad208cbea5f 199 pid_y.setSetPoint(y_point);
skouki 0:3ad208cbea5f 200
skouki 0:3ad208cbea5f 201 pid_spin.reset();
skouki 0:3ad208cbea5f 202 pid_spin.setTunings(5.0,1.0,0.000001);
skouki 0:3ad208cbea5f 203 pid_spin.setInputLimits(-180.0,180.0);
skouki 0:3ad208cbea5f 204 pid_spin.setOutputLimits(-0.5,0.5);
skouki 0:3ad208cbea5f 205 pid_spin.setBias(0);
skouki 0:3ad208cbea5f 206 pid_spin.setMode(1);
skouki 0:3ad208cbea5f 207 pid_spin.setSetPoint(0.0);
skouki 0:3ad208cbea5f 208
skouki 0:3ad208cbea5f 209 mode = 2;
skouki 0:3ad208cbea5f 210 }
skouki 0:3ad208cbea5f 211
skouki 0:3ad208cbea5f 212 if(IR0.get_Averagingdistance()<=20&&mode == 2){
skouki 0:3ad208cbea5f 213 dai_x = m.getOutX();
skouki 0:3ad208cbea5f 214 mode = 0xff;
skouki 0:3ad208cbea5f 215 }
skouki 0:3ad208cbea5f 216
skouki 0:3ad208cbea5f 217 if(instruction_mode == 3&&mode == 0xff){
skouki 0:3ad208cbea5f 218 pid_x.reset();
skouki 0:3ad208cbea5f 219 pid_x.setTunings(3.0,1.0,0.000001);
skouki 0:3ad208cbea5f 220 pid_x.setInputLimits(dai_x - 1000.0,dai_x + 1000.0);
skouki 0:3ad208cbea5f 221 pid_x.setOutputLimits(-1.0,1.0);
skouki 0:3ad208cbea5f 222 pid_x.setBias(0);
skouki 0:3ad208cbea5f 223 pid_x.setMode(1);
skouki 0:3ad208cbea5f 224 pid_x.setSetPoint(dai_x);
skouki 0:3ad208cbea5f 225
skouki 0:3ad208cbea5f 226 pid_spin.reset();
skouki 0:3ad208cbea5f 227 pid_spin.setTunings(5.0,1.0,0.000001);
skouki 0:3ad208cbea5f 228 pid_spin.setInputLimits(-180.0,180.0);
skouki 0:3ad208cbea5f 229 pid_spin.setOutputLimits(-0.5,0.5);
skouki 0:3ad208cbea5f 230 pid_spin.setBias(0);
skouki 0:3ad208cbea5f 231 pid_spin.setMode(1);
skouki 0:3ad208cbea5f 232 pid_spin.setSetPoint(0.0);
skouki 0:3ad208cbea5f 233
skouki 0:3ad208cbea5f 234 mode = 3;
skouki 0:3ad208cbea5f 235 }
skouki 0:3ad208cbea5f 236
skouki 0:3ad208cbea5f 237 if(IR0.get_Averagingdistance()>=50&&mode == 3){
skouki 0:3ad208cbea5f 238 dai_low_y = m.getOutY();
skouki 0:3ad208cbea5f 239
skouki 0:3ad208cbea5f 240 pid_x.reset();
skouki 0:3ad208cbea5f 241 pid_x.setTunings(3.0,1.0,0.000001);
skouki 0:3ad208cbea5f 242 pid_x.setInputLimits(dai_x - 1000.0,dai_x + 1000.0);
skouki 0:3ad208cbea5f 243 pid_x.setOutputLimits(-1.0,1.0);
skouki 0:3ad208cbea5f 244 pid_x.setBias(0);
skouki 0:3ad208cbea5f 245 pid_x.setMode(1);
skouki 0:3ad208cbea5f 246 pid_x.setSetPoint(dai_x);
skouki 0:3ad208cbea5f 247
skouki 0:3ad208cbea5f 248 pid_y.reset();
skouki 0:3ad208cbea5f 249 pid_y.setTunings(1.0,1.0,0.000001);
skouki 0:3ad208cbea5f 250 pid_y.setInputLimits(dai_low_y + gap - 1000.0 ,dai_low_y + gap + 1000.0);
skouki 0:3ad208cbea5f 251 pid_y.setOutputLimits(-1.0,1.0);
skouki 0:3ad208cbea5f 252 pid_y.setBias(0);
skouki 0:3ad208cbea5f 253 pid_y.setMode(1);
skouki 0:3ad208cbea5f 254 pid_y.setSetPoint(dai_low_y + gap);
skouki 0:3ad208cbea5f 255
skouki 0:3ad208cbea5f 256 pid_spin.reset();
skouki 0:3ad208cbea5f 257 pid_spin.setTunings(5.0,1.0,0.000001);
skouki 0:3ad208cbea5f 258 pid_spin.setInputLimits(-180.0,180.0);
skouki 0:3ad208cbea5f 259 pid_spin.setOutputLimits(-0.5,0.5);
skouki 0:3ad208cbea5f 260 pid_spin.setBias(0);
skouki 0:3ad208cbea5f 261 pid_spin.setMode(1);
skouki 0:3ad208cbea5f 262 pid_spin.setSetPoint(0.0);
skouki 0:3ad208cbea5f 263
skouki 0:3ad208cbea5f 264 mode = 4;
skouki 0:3ad208cbea5f 265 }
skouki 0:3ad208cbea5f 266
skouki 0:3ad208cbea5f 267 if(Y_power < 0.05 && Y_power > -0.05 && mode == 4){
skouki 0:3ad208cbea5f 268 mode = 0xff;
skouki 0:3ad208cbea5f 269 }
skouki 0:3ad208cbea5f 270
skouki 0:3ad208cbea5f 271 if(mode == 1)mode1();
skouki 0:3ad208cbea5f 272 if(mode == 2)mode2();
skouki 0:3ad208cbea5f 273 if(mode == 3)mode3();
skouki 0:3ad208cbea5f 274 if(mode == 4)mode4();
skouki 0:3ad208cbea5f 275
skouki 0:3ad208cbea5f 276 omni.computeXY(Y_power,-X_power,-spin_power);
skouki 0:3ad208cbea5f 277
skouki 0:3ad208cbea5f 278 for(int i = 0; i < 4; i++){
skouki 0:3ad208cbea5f 279 omni_power[i] = 0.0;
skouki 0:3ad208cbea5f 280 omni_power[i] = omni.wheel[i];
skouki 0:3ad208cbea5f 281 motor[i].setSpeed(omni_power[i]);
skouki 0:3ad208cbea5f 282 }
skouki 0:3ad208cbea5f 283
skouki 0:3ad208cbea5f 284 }
skouki 0:3ad208cbea5f 285 }