Revision 1:697097e548f9, committed 2019-10-02

Comitter:

skouki

Date:

Wed Oct 02 10:00:19 2019 +0000

Parent:

0:f68b460de813

Commit message:

v6;

Changed in this revision

diff -r f68b460de813 -r 697097e548f9 main.cpp
--- a/main.cpp	Fri Sep 13 02:17:39 2019 +0000
+++ b/main.cpp	Wed Oct 02 10:00:19 2019 +0000
@@ -1,40 +1,89 @@
 #include"mbed.h"
 #include"SerialMultiByte.h"
 #include"pin_config.h"
+#include"IRsensor.h"
+#include"QEI.h"
+#include"PID.h"
+#include"ikarashiMDC.h"
+
+#define POINT 22380
 
 SerialMultiByte sita_s(S_SERIALTX,S_SERIALRX);
 SerialMultiByte ue_s(U_SERIALTX,U_SERIALRX);
-DigitalIn start(USER_BUTTON);
+DigitalIn start(STARTPIN);
 Serial pc(USBTX,USBRX,115200);
 
 DigitalOut debug_led_0(LED0);
 DigitalOut debug_led_1(LED1);
 DigitalOut debug_led_2(LED2);
-DigitalIn debug_button(PC_4);
+DigitalIn mode_r_b(MODE_R_B);
+PID pid(5.0,3.0,0.000001,0.001);
 
+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)
+};
+
+QEI wheel_2(QEI_1_1,QEI_1_2, NC, 500,QEI::X4_ENCODING);
+QEI wheel_1(QEI_3_1,QEI_3_2, NC, 500,QEI::X4_ENCODING);
+/*
+IRsensor IR0(IR_0);
+IRsensor IR1(IR_1);
+IRsensor IR2(IR_2);
+*/
 int mode=0;
 int u_mode=0,s_mode=0,m_mode=0;
 unsigned char itidata[4];
 int X_,Y_;
 int data_a;
+double power[4];
+bool end_flag = true;
+bool down_flag = false;
+bool debugmode = false;
+int height_point = POINT;
+double plus_power;
+int field;
 
 void to_sita(){
-  unsigned char data[5];
+  unsigned char data[6];
   unsigned char getdata[1];
   data[0] = mode;
    for(int i=0;i<4;i++){
      data[i+1] = itidata[i];
   }
-  sita_s.sendData(data,5);
+  data[5] = field;
+  sita_s.sendData(data,6);
   sita_s.getData(getdata);
   s_mode = getdata[0];
 }
+/*
+void ir_checker(){
+  float ir_0 = IR0.get_Averagingdistance();
+  float ir_1 = IR1.get_Averagingdistance();
+  float ir_2 = IR2.get_Averagingdistance();
+  pc.printf("%.2f\\%.2f\\%.2f\\",ir_0,ir_1,ir_2);
 
+  data_a = 2;
+  if(ir_0 <= 40.0) {
+    data_a = 1;
+    return;
+  }
+  if(ir_2 <= 40.0){
+    data_a = 3;
+    return;
+  }
+  data_a = 0;
+}
+*/
 void to_ue(){
-  unsigned char data[1];
+  unsigned char data[2];
   unsigned char getdata[5];
   data[0] = mode;
-  ue_s.sendData(data,1);
+  data[1] = field;
+  ue_s.sendData(data,2);
   ue_s.getData(getdata);
   u_mode=getdata[0];
   for(int i=0;i<4;i++){
@@ -42,23 +91,112 @@
   }
 }
 
+void syoukou()
+{  
 
+  if(debugmode)return;
+  if(end_flag&&!down_flag){
+    if(height_point >= wheel_1.getPulses())power[0] = 0.8;
+    else power[0] = 0.0;
+    if(height_point >= wheel_2.getPulses()){
+      pid.setProcessValue(wheel_1.getPulses() - wheel_2.getPulses());
+      plus_power = pid.compute();
+      power[1] = 0.8 - plus_power;
+    }
+    else power[1] = 0.0;
+    if((height_point <= wheel_1.getPulses())&&(height_point <= wheel_2.getPulses()))down_flag = true;
+  }
+  if(!end_flag){
+    power[0] += 0.0;
+    power[1] += 0.0;
+  }
+  if(down_flag){
+    if((height_point-3000) < wheel_1.getPulses())power[0] -= 0.6;
+    else power[0] = 0.0;
+    if((height_point-3000) < wheel_2.getPulses())power[1] -= 0.6;
+    else power[1] = 0.0;
+    if(power[0]+power[1] == 0.0){
+        down_flag = false;
+        end_flag = false;
+    }
+  }
+}
+void iti_checker()
+{
+  int X_;
+  if(itidata[1]>=(1<<7)){
+    itidata[1]-=(1<<7);
+    X_=-1*(itidata[0]+(itidata[1]<<8));
+  }
+  else X_= itidata[0]+(itidata[1]<<8);
+  
+  if(X_ <= -1000)motor[2].setSpeed(-0.2);
+  else motor[2].setSpeed(0.0);    
+    
+}
 int main()
 {
     sita_s.setHeaders('A','Z');
     sita_s.startReceive(1);
     ue_s.setHeaders('A','Z');
     ue_s.startReceive(5);
-    debug_button.mode(PullDown);
+    /*
+    IR0.startAveraging(5);
+    IR1.startAveraging(5);
+    IR2.startAveraging(5);*/
+
+    pid.setInputLimits(-10000.0,10000.0);
+    pid.setOutputLimits(-1.0,1.0);
+    pid.setBias(0);
+    pid.setMode(1);
+    pid.setSetPoint(0.0);
+    
+    field = mode_r_b.read();
+    debug_led_1 = field;
+    debug_led_2 = !field;
     while(1)
     {
+        for(int i = 0;i < 2;i++)power[i] = 0.0;
         debug_led_0 = !debug_led_0;
+        //ir_checker();
         to_ue();
         to_sita();
-        if(!start)mode = 1;
+        iti_checker();
+        if(start.read() == 0)mode = 1;
         if(mode==1&&s_mode ==2)mode = 2;
-        if(mode == 2&&u_mode == 0xff &&s_mode == 0xff)mode = 3;
-        if(mode == 3&&u_mode == 0xff &&s_mode == 0xff)mode = 5;
+        if(mode == 2&&(u_mode == 0xff || s_mode == 0xff))mode = 0xff - 1;
+        if((mode == 0xff - 1)&&u_mode == 0xff && s_mode == 0xff)mode = 3;
+
+        
+        if(mode == 3&&u_mode == 3 &&s_mode == 3)syoukou();
+        if(mode == 3&&u_mode == 3 &&s_mode == 4)syoukou();
+        if(mode == 3&&u_mode == 4 &&s_mode == 3)syoukou();
+        if(mode == 3&&u_mode == 4 &&s_mode == 4)syoukou();
+        
+        if(mode == 3&&u_mode == 4 &&s_mode == 4&&!end_flag)mode = 5;
+        
+        if(mode == 1){
+                if(wheel_1.getPulses() <= 1000)power[0] += 0.5;
+                else power[0] = 0;
+                if(wheel_2.getPulses() <= 1000)power[1] += 0.5;
+                else power[1] = 0;
+        }
+        /*
+        if(u_mode == 6&&s_mode==6){
+            if((height_point-4000) < wheel_1.getPulses())power[0] -= 0.6;
+            else power[0] = 0.0;
+            if((height_point-4000) < wheel_2.getPulses())power[1] -= 0.6;
+            else power[1] = 0.0;
+        }
+        */
+        
+        motor[0].setSpeed(power[0]);
+        motor[1].setSpeed(power[1]);
+
+        
+        pc.printf("%d,%d,%d,%d,%.2f,%.2f,%d\n\r",s_mode,u_mode,wheel_1.getPulses(),wheel_2.getPulses(),power[0],power[1],mode_r_b.read());
+        //pc.printf("%d\n\r",data_a);
+
 
 
     }

diff -r f68b460de813 -r 697097e548f9 pin_config.h
--- a/pin_config.h	Fri Sep 13 02:17:39 2019 +0000
+++ b/pin_config.h	Wed Oct 02 10:00:19 2019 +0000
@@ -27,5 +27,10 @@
 #define QEI_4_1 PB_1
 #define QEI_4_2 PB_15
 
+#define STARTPIN PB_7
+
+//#define MODE_R_B PA_15
+#define MODE_R_B PC_13
+
 #endif