Revision 1:64871263444f, committed 2019-10-02

Comitter:

skouki

Date:

Wed Oct 02 09:59:32 2019 +0000

Parent:

0:3ad208cbea5f

Commit message:

v6;

Changed in this revision

--- a/main.cpp	Fri Sep 13 02:19:03 2019 +0000
+++ b/main.cpp	Wed Oct 02 09:59:32 2019 +0000
@@ -8,8 +8,9 @@
 #include"PID.h"
 #include"IRsensor.h"
 
-#define YPOINT 4000
-#define GAP 0
+#define YPOINT 6400
+#define GAP 15
+#define MTOU 300
 
 Serial serial(MDCTX,MDCRX,115200);
 ikarashiMDC motor[]={
@@ -18,7 +19,7 @@
     ikarashiMDC(1,2,SM,&serial),
     ikarashiMDC(1,3,SM,&serial)
 };
-PositionController position_control_1(500,500,0.1,0.01,0.8);
+PositionController position_control_1(1000,1000,0.3,0.01,0.3);
 
 OmniWheel omni(4);
 SerialMultiByte s(SERIALTX,SERIALRX);
@@ -28,12 +29,13 @@
 PID pid_y(0,0,0,0.001);
 Serial pc(USBTX,USBRX,115200);
 
-DigitalIn debug_button(USER_BUTTON);
+DigitalIn an(USER_BUTTON);
 DigitalOut debug_led_0(LED_0);
 DigitalOut debug_led_1(LED_2);
 DigitalOut debug_led_2(LED_1);
 
 IRsensor IR0(IR_0);
+IRsensor IR1(IR_1);
 
 int mode;
 int instruction_mode;
@@ -44,6 +46,9 @@
 int X_,Y_;
 double dai_x,dai_low_y;
 int gap = GAP;
+double ir_distance;
+int data_a;
+int m_to_u = MTOU;
 
 void set_up()
 {
@@ -54,9 +59,10 @@
   omni.wheel[3].setRadian(PIII + theta);
 
   s.setHeaders('A','Z');
-  s.startReceive(5);
+  s.startReceive(6);
 
   IR0.startAveraging(5);
+  IR1.startAveraging(5);
 
 }
 
@@ -65,12 +71,14 @@
   pid_x.setProcessValue(m.getOutX());
   X_power += pid_x.compute();
 
-  position_control_1.compute(0.0,m.getOutY());
+  position_control_1.compute(1,m.getOutY());
   Y_power += position_control_1.getVelocityY();
 
-  pid_y.setProcessValue(m.getOutY() - Y_);
+  pid_y.setProcessValue(m.getOutY() - Y_ + 200);
   Y_power += pid_y.compute();
 
+  if(Y_power <= 0.0)Y_power = 0.0;
+
   pid_spin.setProcessValue(m.getjyroAngle());
   spin_power = pid_spin.compute();
 
@@ -78,13 +86,18 @@
 
 void mode2()
 {
-  X_power -= 0.3;
+  if(data_a)X_power -= 0.3;
+  else X_power += 0.3;
 
   pid_x.setProcessValue(m.getOutX() - X_);
   X_power += pid_x.compute();
 
   pid_y.setProcessValue(m.getOutY());
   Y_power += pid_y.compute();
+  
+  if(y_point == 0){
+      if(Y_power <= 0.0)Y_power = 0.0;
+  }
 
   pid_spin.setProcessValue(m.getjyroAngle());
   spin_power = pid_spin.compute();
@@ -96,7 +109,11 @@
   pid_x.setProcessValue(m.getOutX());
   X_power += pid_x.compute();
 
-  Y_power -= 0.3;
+  pid_y.setProcessValue(Y_ - m.getOutY());
+  Y_power -= pid_y.compute();
+  
+  
+  if(Y_power <= 0.0)Y_power = 0.0;
 
   pid_spin.setProcessValue(m.getjyroAngle());
   spin_power = pid_spin.compute();
@@ -108,18 +125,60 @@
   pid_x.setProcessValue(m.getOutX());
   X_power += pid_x.compute();
 
-  pid_y.setProcessValue(m.getOutY());
-  Y_power += pid_y.compute();
+  pid_y.setProcessValue(Y_ - m.getOutY());
+  Y_power -= pid_y.compute();
+  
+  if(Y_power <= 0.0)Y_power = 0.0;
+
 
   pid_spin.setProcessValue(m.getjyroAngle());
   spin_power = pid_spin.compute();
 
 }
 
+void mode5()
+{
+  if(data_a)X_power -= 0.3;
+  else X_power += 0.3;
+
+
+  pid_x.setProcessValue(m.getOutX() - X_);
+  X_power += pid_x.compute();
+
+
+  pid_y.setProcessValue(Y_ - m.getOutY());
+  Y_power -= pid_y.compute();
+
+  pid_spin.setProcessValue(m.getjyroAngle());
+  spin_power = pid_spin.compute();
+
+}
+
+void mode6()
+{
+  if(data_a)X_power -= 0.3;
+  else X_power += 0.3;
+
+
+  pid_x.setProcessValue(m.getOutX() - X_);
+  X_power += pid_x.compute();
+
+
+  pid_y.setProcessValue(Y_ - m.getOutY());
+  Y_power -= pid_y.compute();
+
+  pid_spin.setProcessValue(m.getjyroAngle());
+  spin_power = pid_spin.compute();
+
+  if(m.getjyroAngle() <=-10.0)Y_power += 0.5;
+
+
+}
+
 void to_main()
 {
   unsigned char data[1];
-  unsigned char getdata[5];
+  unsigned char getdata[6];
   data[0] = mode;
   s.sendData(data,1);
   s.getData(getdata);
@@ -136,16 +195,24 @@
     Y_=-1*(getdata[3]+(getdata[4]<<8));
   }
   else Y_= getdata[3]+(getdata[4]<<8);
+  data_a = getdata[5];
   if(instruction_mode!=0)debug_led_1 = !debug_led_1;
 }
 int main()
 {
   set_up();
+    an.mode(PullUp);
   while(true){
     debug_led_0 = !debug_led_0;
+    if(m.getjyroAngle() <= 1.0 && m.getjyroAngle() >= -1.0){
+      debug_led_2 = 1;
+    }
+    else debug_led_2 = 0;
     X_power = 0.0;
     Y_power = 0.0;
     spin_power = 0.0;
+    if(data_a)ir_distance = IR0.get_Averagingdistance();
+    else ir_distance = IR1.get_Averagingdistance();
 
     to_main();
 
@@ -162,7 +229,7 @@
       position_control_1.targetXY(1,int(y_point));
 
       pid_y.reset();
-      pid_y.setTunings(1.0,1.0,0.000001);
+      pid_y.setTunings(7.0,1.0,0.000001);
       pid_y.setInputLimits(-1000.0,1000.0);
       pid_y.setOutputLimits(-1.0,1.0);
       pid_y.setBias(0);
@@ -180,10 +247,10 @@
       mode = 1;
     }
 
-    if((m.getOutY() >= (y_point - 50)) && mode == 1){
+    if((m.getOutY() >= (y_point - 260)) && mode == 1){
 
       pid_x.reset();
-      pid_x.setTunings(1.0,1.0,0.000001);
+      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);
@@ -191,12 +258,12 @@
       pid_x.setSetPoint(0.0);
 
       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.setTunings(5.0,1.0,0.000001);
+      pid_y.setInputLimits(y_point - 150 - 1000.0,y_point - 150 + 1000.0);
       pid_y.setOutputLimits(-1.0,1.0);
       pid_y.setBias(0);
       pid_y.setMode(1);
-      pid_y.setSetPoint(y_point);
+      pid_y.setSetPoint(y_point - 200);
 
       pid_spin.reset();
       pid_spin.setTunings(5.0,1.0,0.000001);
@@ -209,7 +276,7 @@
       mode = 2;
     }
 
-    if(IR0.get_Averagingdistance()<=20&&mode == 2){
+    if(((ir_distance<=10&&mode == 2)||instruction_mode == 0xff - 1) && mode != 0xff){
       dai_x = m.getOutX();
       mode = 0xff;
     }
@@ -223,6 +290,14 @@
       pid_x.setMode(1);
       pid_x.setSetPoint(dai_x);
 
+      pid_y.reset();
+      pid_y.setTunings(10.0,1.0,0.000001);
+      pid_y.setInputLimits(m_to_u-1000.0,m_to_u+1000.0);
+      pid_y.setOutputLimits(-0.3,0.3);
+      pid_y.setBias(0);
+      pid_y.setMode(1);
+      pid_y.setSetPoint(m_to_u);
+
       pid_spin.reset();
       pid_spin.setTunings(5.0,1.0,0.000001);
       pid_spin.setInputLimits(-180.0,180.0);
@@ -234,7 +309,7 @@
       mode = 3;
     }
 
-    if(IR0.get_Averagingdistance()>=50&&mode == 3){
+    if(ir_distance>=20&&mode == 3){
       dai_low_y = m.getOutY();
 
       pid_x.reset();
@@ -245,14 +320,23 @@
       pid_x.setMode(1);
       pid_x.setSetPoint(dai_x);
 
+ //     pid_y.reset();
+//      pid_y.setTunings(10.0,1.0,0.000001);
+//      pid_y.setInputLimits(dai_low_y - gap - 1000.0 ,dai_low_y - gap + 1000.0);
+//      pid_y.setOutputLimits(-1.0,1.0);
+//      pid_y.setBias(0);
+//      pid_y.setMode(1);
+//      pid_y.setSetPoint(dai_low_y - gap);
+
+      m_to_u = MTOU + 40 ;
       pid_y.reset();
-      pid_y.setTunings(1.0,1.0,0.000001);
-      pid_y.setInputLimits(dai_low_y + gap - 1000.0 ,dai_low_y + gap + 1000.0);
-      pid_y.setOutputLimits(-1.0,1.0);
+      pid_y.setTunings(10.0,1.0,0.000001);
+      pid_y.setInputLimits(m_to_u-1000.0,m_to_u+1000.0);
+      pid_y.setOutputLimits(-0.5,0.5);
       pid_y.setBias(0);
       pid_y.setMode(1);
-      pid_y.setSetPoint(dai_low_y + gap);
-
+      pid_y.setSetPoint(m_to_u);
+      
       pid_spin.reset();
       pid_spin.setTunings(5.0,1.0,0.000001);
       pid_spin.setInputLimits(-180.0,180.0);
@@ -264,14 +348,85 @@
       mode = 4;
     }
 
-    if(Y_power < 0.05 && Y_power > -0.05 && mode == 4){
-      mode = 0xff;
+    if(instruction_mode == 5&&mode == 4){
+      //gap = GAP - 30;
+
+      pid_x.reset();
+      pid_x.setTunings(5.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);
+
+//      pid_y.reset();
+//      pid_y.setTunings(5.0,1.0,0.000001);
+//      pid_y.setInputLimits(dai_low_y - gap - 1000.0 ,dai_low_y - gap + 1000.0);
+//      pid_y.setOutputLimits(-1.0,1.0);
+//      pid_y.setBias(0);
+//      pid_y.setMode(1);
+//      pid_y.setSetPoint(dai_low_y - gap);
+
+      pid_y.reset();
+      pid_y.setTunings(5.0,1.0,0.000001);
+      pid_y.setInputLimits(m_to_u-1000.0,m_to_u+1000.0);
+      pid_y.setOutputLimits(-1.0,1.0);
+      pid_y.setBias(0);
+      pid_y.setMode(1);
+      pid_y.setSetPoint(m_to_u);
+      
+      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 = 5;
     }
+/*
+    if(m.getOutX() <= -1600&&mode == 5){
+
+      //gap = GAP - 50;
+
+      pid_x.reset();
+      pid_x.setTunings(1.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);
+
+      pid_y.reset();
+      pid_y.setTunings(8.0,1.0,0.000001);
+      pid_y.setInputLimits(dai_low_y - gap - 1000.0 ,dai_low_y - gap + 1000.0);
+      pid_y.setOutputLimits(-1.0,1.0);
+      pid_y.setBias(0);
+      pid_y.setMode(1);
+      pid_y.setSetPoint(dai_low_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 = 6;
+    }
+    */
+
+    if(an.read()==0)y_point = 0;
 
     if(mode == 1)mode1();
     if(mode == 2)mode2();
     if(mode == 3)mode3();
     if(mode == 4)mode4();
+    if(mode == 5)mode5();
+    if(mode == 6)mode6();
+    if(m.getOutX() <= -3400){X_power = 0.0;Y_power = 0.0;}
 
     omni.computeXY(Y_power,-X_power,-spin_power);
 
@@ -280,6 +435,8 @@
       omni_power[i] = omni.wheel[i];
       motor[i].setSpeed(omni_power[i]);
     }
+    //pc.printf("%.2f,%.2f,%d,%d,%.2f,%.2f\n\r",m.getOutX(),m.getOutY(),X_,Y_,m.getjyroAngle(),ir_distance);
+
 
   }
 }