Diff: main.cpp

Revision:

0:76663617eca3

diff -r 000000000000 -r 76663617eca3 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Sep 13 02:15:30 2019 +0000
@@ -0,0 +1,274 @@
+#include"mbed.h"
+#include"measuring_wheel.h"
+#include"ikarashiMDC.h"
+#include"position_controller.h"
+#include"omni_wheel.h"
+#include"pin_config.h"
+#include"SerialMultiByte.h"
+#include"PID.h"
+#include"IRsensor.h"
+
+#define YPOINT 4000
+#define GAP 0
+
+Serial serial(MDCTX,MDCRX,115200);
+ikarashiMDC motor[]={
+    ikarashiMDC(1,0,SM,&serial),
+    ikarashiMDC(1,1,SM,&serial),
+    ikarashiMDC(1,2,SM,&serial),
+    ikarashiMDC(1,3,SM,&serial)
+};
+PositionController position_control_1(500,500,0.1,0.01,0.8);
+
+OmniWheel omni(4);
+SerialMultiByte s(SERIALTX,SERIALRX);
+MeasuringWheel m(QEI1_1,QEI1_2,QEI4_1,QEI4_2,QEI3_1,QEI3_2,R1370TX,R1370RX);
+PID pid_spin(0,0,0,0.001);
+PID pid_x(0,0,0,0.001);
+PID pid_y(0,0,0,0.001);
+Serial pc(USBTX,USBRX,115200);
+
+DigitalIn debug_button(USER_BUTTON);
+DigitalOut debug_led_0(LED_0);
+DigitalOut debug_led_1(LED_2);
+DigitalOut debug_led_2(LED_1);
+DigitalOut emergency_stop(EMERGENCY_STOP);
+
+IRsensor IR0(IR_0);
+
+int mode;
+int instruction_mode;
+double omni_power[4];
+double X_power,Y_power;
+double spin_power;
+float y_point = YPOINT;
+
+double dai_x,dai_high_y;
+int gap = GAP;
+
+void set_up()
+{
+  float theta = PIII / 4;
+  omni.wheel[0].setRadian(PIII - theta);
+  omni.wheel[1].setRadian(theta);
+  omni.wheel[2].setRadian(-theta);
+  omni.wheel[3].setRadian(PIII + theta);
+
+  s.setHeaders('A','Z');
+  s.startReceive(1);
+
+  IR0.startAveraging(5);
+
+}
+
+void mode1()
+{
+  pid_x.setProcessValue(m.getOutX());
+  X_power += pid_x.compute();
+
+  position_control_1.compute(0.0,m.getOutY());
+  Y_power += position_control_1.getVelocityY();
+
+  pid_spin.setProcessValue(m.getjyroAngle());
+  spin_power = pid_spin.compute();
+
+}
+
+void mode2()
+{
+  X_power -= 0.3;
+
+  pid_y.setProcessValue(m.getOutY());
+  Y_power += pid_y.compute();
+
+  pid_spin.setProcessValue(m.getjyroAngle());
+  spin_power = pid_spin.compute();
+
+}
+
+void mode3()
+{
+  pid_x.setProcessValue(m.getOutX());
+  X_power += pid_x.compute();
+
+  Y_power += 0.3;
+
+  pid_spin.setProcessValue(m.getjyroAngle());
+  spin_power = pid_spin.compute();
+
+}
+
+void mode4()
+{
+  pid_x.setProcessValue(m.getOutX());
+  X_power += pid_x.compute();
+
+  pid_y.setProcessValue(m.getOutY());
+  Y_power += pid_y.compute();
+
+  pid_spin.setProcessValue(m.getjyroAngle());
+  spin_power = pid_spin.compute();
+
+}
+
+void to_main()
+{
+  unsigned char data[5];
+  unsigned char getdata[1];
+  int X_ = m.getOutX();
+  int Y_ = m.getOutY();
+  data[0] = mode;
+  if(X_>0){
+      data[1] = X_&0x00ff;
+      data[2] = (X_>>8)&0x00ff;
+  }
+  else{
+      X_*=-1;
+      data[1] = X_&0x00ff;
+      data[2] = ((X_>>8)&0x00ff )+ (1<<7);
+  }
+  if(Y_>0){
+      data[3] = Y_&0x00ff;
+      data[4] = (Y_>>8)&0x00ff;
+  }
+  else{
+      Y_*=-1;
+      data[3] = Y_&0x00ff;
+      data[4] = ((Y_>>8)&0x00ff )+ (1<<7);
+  }
+  s.sendData(data,5);
+  s.getData(getdata);
+  instruction_mode = getdata[0];
+  if(instruction_mode)debug_led_1 = !debug_led_1;
+}
+
+int main()
+{
+  set_up();
+  emergency_stop = 1;
+  while(true){
+    debug_led_0 = !debug_led_0;
+    X_power = 0.0;
+    Y_power = 0.0;
+    spin_power = 0.0;
+
+    to_main();
+
+    if(instruction_mode == 1&&mode == 0){
+
+      pid_x.reset();
+      pid_x.setTunings(3.0,1.0,0.000001);
+      pid_x.setInputLimits(-1000.0,1000.0);
+      pid_x.setOutputLimits(-1.0,1.0);
+      pid_x.setBias(0);
+      pid_x.setMode(1);
+      pid_x.setSetPoint(0.0);
+
+      position_control_1.targetXY(1,int(y_point));
+
+      pid_spin.reset();
+      pid_spin.setTunings(5.0,1.0,0.000001);
+      pid_spin.setInputLimits(-180.0,180.0);
+      pid_spin.setOutputLimits(-0.5,0.5);
+      pid_spin.setBias(0);
+      pid_spin.setMode(1);
+      pid_spin.setSetPoint(0.0);
+
+      mode = 1;
+    }
+
+    if(instruction_mode == 2&&mode == 1){
+
+      pid_y.reset();
+      pid_y.setTunings(1.0,1.0,0.000001);
+      pid_y.setInputLimits(y_point - 1000.0 , y_point + 1000.0);
+      pid_y.setOutputLimits(-1.0,1.0);
+      pid_y.setBias(0);
+      pid_y.setMode(1);
+      pid_y.setSetPoint(y_point);
+
+      pid_spin.reset();
+      pid_spin.setTunings(5.0,1.0,0.000001);
+      pid_spin.setInputLimits(-180.0,180.0);
+      pid_spin.setOutputLimits(-0.5,0.5);
+      pid_spin.setBias(0);
+      pid_spin.setMode(1);
+      pid_spin.setSetPoint(0.0);
+
+      mode = 2;
+    }
+
+    if(IR0.get_Averagingdistance()<=20&&mode == 2){
+      dai_x = m.getOutX();
+      mode = 0xff;
+    }
+
+    if(instruction_mode == 3&&mode == 0xff){
+      pid_x.reset();
+      pid_x.setTunings(3.0,1.0,0.000001);
+      pid_x.setInputLimits(dai_x - 1000.0,dai_x + 1000.0);
+      pid_x.setOutputLimits(-1.0,1.0);
+      pid_x.setBias(0);
+      pid_x.setMode(1);
+      pid_x.setSetPoint(dai_x);
+
+      pid_spin.reset();
+      pid_spin.setTunings(5.0,1.0,0.000001);
+      pid_spin.setInputLimits(-180.0,180.0);
+      pid_spin.setOutputLimits(-0.5,0.5);
+      pid_spin.setBias(0);
+      pid_spin.setMode(1);
+      pid_spin.setSetPoint(0.0);
+
+      mode = 3;
+    }
+
+    if(IR0.get_Averagingdistance()>=50&&mode == 3){
+      dai_high_y = m.getOutY();
+
+      pid_x.reset();
+      pid_x.setTunings(3.0,1.0,0.000001);
+      pid_x.setInputLimits(dai_x - 1000.0,dai_x + 1000.0);
+      pid_x.setOutputLimits(-1.0,1.0);
+      pid_x.setBias(0);
+      pid_x.setMode(1);
+      pid_x.setSetPoint(dai_x);
+
+      pid_y.reset();
+      pid_y.setTunings(1.0,1.0,0.000001);
+      pid_y.setInputLimits(dai_high_y + gap - 1000.0 ,dai_high_y + gap + 1000.0);
+      pid_y.setOutputLimits(-1.0,1.0);
+      pid_y.setBias(0);
+      pid_y.setMode(1);
+      pid_y.setSetPoint(dai_high_y + gap);
+
+      pid_spin.reset();
+      pid_spin.setTunings(5.0,1.0,0.000001);
+      pid_spin.setInputLimits(-180.0,180.0);
+      pid_spin.setOutputLimits(-0.5,0.5);
+      pid_spin.setBias(0);
+      pid_spin.setMode(1);
+      pid_spin.setSetPoint(0.0);
+
+      mode = 4;
+    }
+
+    if(Y_power < 0.05 && Y_power > -0.05 && mode == 4){
+      mode = 0xff;
+    }
+
+    if(mode == 1)mode1();
+    if(mode == 2)mode2();
+    if(mode == 3)mode3();
+    if(mode == 4)mode4();
+
+    omni.computeXY(Y_power,-X_power,-spin_power);
+
+    for(int i = 0; i < 4; i++){
+      omni_power[i] = 0.0;
+      omni_power[i] = omni.wheel[i];
+      motor[i].setSpeed(omni_power[i]);
+    }
+
+  }
+}