Diff: main.cpp

Revision:

3:6223efea43fe

Parent:

2:c9de02d6d154

Child:

4:5591d3c8a761

--- a/main.cpp	Mon Aug 28 01:39:14 2017 +0000
+++ b/main.cpp	Mon Aug 28 07:27:23 2017 +0000
@@ -3,14 +3,25 @@
 #include "cal_PID.hpp"
 #include "timeBaseTrapezoidalMotionCal.h"
 
-const float omega_max = 1.0 * 2 * 3.14159265f;
-const float wrap_radius = 1.5f;
-const float wrap_speed = 2.0f;
+const float omuni_speed[4] = {0, 1.0, 1.7, 3.0};
+const float omega_max = 0.7 * 2 * 3.14159265f;
+const float wrap_radius = 1.0f;
+const float wrap_speed = 1.7f;
 const int8_t armDuty[] = {-50, -50};
 const signed int spearDuty = 127;
-const int32_t lift_preset[4] = {550, 1033, 1516, 2000}; // 12bit 0 ~ 4095
-const int32_t lift_max = 1980;
-const int32_t lift_min = 550;
+const int32_t lift_preset_min = 420;
+const int32_t lift_preset_max = 1800;
+const int32_t lift_preset[4] = {
+    lift_preset_min,
+    (lift_preset_max - lift_preset_min) / 3.0 + lift_preset_min,
+    (lift_preset_max - lift_preset_min) * 2.0 / 3.0 + lift_preset_min,
+    lift_preset_max,
+    }; // 12bit 0 ~ 4095
+const int32_t lift_max = lift_preset_max;
+const int32_t lift_min = lift_preset_min;
+const int32_t lift_up_speed = 30;
+const int32_t lift_down_speed = 30;
+
 
 const int omuniAddr[] = {0x10, 0x12, 0x14}; // 0000 , 1000 , 0100
 const int armAddr[] = {0x16, 0x18};    // 1100 , 0010
@@ -27,12 +38,10 @@
 
 int lift_targ = lift_preset[0];
 int lift_stepTarg;
-int lift_stepOrigin;
+bool lift_stepMoving = false;
 bool first_receive = true;
 int past_lift_gray = 0b00;
 int lift_inc = 0;
-int lift_task = 0;
-int lift_time = 0;
 
 //DigitalIn button(USER_BUTTON);
 DigitalOut led(LED1);
@@ -45,7 +54,7 @@
 DigitalIn spear_sensor(PC_3);
 Timer mesPeriod;
 cal_pid lift_pid;
-timeBaseTrapezoidalMotionCal lift_motionCal(500, 500, 5000, 70000, -70000);
+
 
 float speed_x, speed_y, omega;
 bool f;
@@ -172,12 +181,13 @@
     
     if(rcTemp.byteNum == 3){ // データは揃った
         
-//        led = !led;
+        led = !led;
         
         ems = RCData[3].emo != 0;
         
         rad = RCData[0].angle * 3.141592 / 8.0;
-        speed = 0.8 * RCData[0].speed;
+        speed = omuni_speed[RCData[0].speed];
+        if(RCData[0].speed == 0)speed = 0;
         speed_x = speed * cos(rad) * -1;
         speed_y = speed * sin(rad) * -1;
         
@@ -211,7 +221,12 @@
             int lift_nextTarg;
             
             lift_currentTarg = lift_targ;
-            lift_nextTarg = lift_currentTarg;
+            if(lift_stepMoving){
+                lift_nextTarg = lift_stepTarg;
+            }
+            else{
+                lift_nextTarg = lift_currentTarg;
+            }
             
             lift_diff = grayDiffer2bit(RCData[2].lift_gray, past_lift_gray);
             for(int num = 0; num < 4; num++){ // 最寄りの段階
@@ -220,29 +235,29 @@
                     lift_nearest = abs(lift_preset[num] - lift_currentTarg);
                 }
             }
-            if(lift_diff == 1){
-                if(lift_preset[lift_nearest_num] >= lift_currentTarg){
-                    if(lift_nearest_num < 3)lift_nextTarg = lift_preset[lift_nearest_num + 1];
-                }
-                else{
-                    lift_nextTarg = lift_preset[lift_nearest_num];
-                }
-            }
-            else if(lift_diff == -1){
-                if(lift_preset[lift_nearest_num] <= lift_currentTarg){
-                    if(lift_nearest_num > 0)lift_nextTarg = lift_preset[lift_nearest_num - 1];
+            if(lift_diff != 0){
+                if(lift_diff == 1){
+                    if(lift_preset[lift_nearest_num] >= lift_currentTarg){p.printf("$$");
+                        if(lift_nearest_num < 3)lift_nextTarg = lift_preset[lift_nearest_num + 1];
+                    }
+                    else{
+                        lift_nextTarg = lift_preset[lift_nearest_num];
+                    }
                 }
-                else{
-                    lift_nextTarg = lift_preset[lift_nearest_num];
+                else if(lift_diff == -1){
+                    if(lift_preset[lift_nearest_num] <= lift_currentTarg){
+                        if(lift_nearest_num > 0)lift_nextTarg = lift_preset[lift_nearest_num - 1];
+                    }
+                    else{
+                        lift_nextTarg = lift_preset[lift_nearest_num];
+                    }
                 }
+                
+                lift_stepTarg = lift_nextTarg;
+                lift_stepMoving = true;
+                                
             }
-            if(lift_task == 0){
-                lift_stepOrigin = lift_currentTarg;
-                lift_stepTarg = lift_nextTarg;
-                lift_motionCal.setTarg(lift_stepTarg - lift_stepOrigin);
-                lift_time = 0;
-                lift_task += 1;
-            }
+            
         }
         past_lift_gray = RCData[2].lift_gray;
         
@@ -269,14 +284,19 @@
 void lift_control(){
     int32_t meter = 0;
     signed int duty = 0;
+    int32_t error = 0;
     
-    if(lift_task != 0){
-        if(lift_targ != lift_stepTarg){
-            lift_targ = lift_motionCal.calSteps(lift_time) + lift_stepOrigin;
-            lift_time += 1;
+    error = lift_stepTarg - lift_targ;
+    if(lift_stepMoving){
+        if(error > lift_up_speed){
+            lift_targ += lift_up_speed; 
+        }
+        else if(error < lift_down_speed * -1){
+            lift_targ -= lift_down_speed;
         }
         else{
-            lift_task = 0;
+            lift_targ = lift_stepTarg;
+            lift_stepMoving = false;
         }
     }
     else{
@@ -289,7 +309,7 @@
     }
     
     meter = lift_meter.read_u16() >> 4;
-    duty = lift_pid.get_pid(meter, lift_targ);
+    duty = lift_pid.get_pid(meter, lift_targ, -1);
     drive_motor(liftAddr, duty);
     
 }
@@ -363,14 +383,14 @@
     p.printf("Hello!\n");
     pc.attach(pc_rx, Serial::RxIrq);
     
-    //i2cMaster.frequency(400000);
+    i2cMaster.frequency(400000);
     
     omuni.set_speed(0.0f, 0.0f);
     omuni.set_pid(0, 3.0f, 0.07f, 0.05f);
     omuni.set_pid(1, 3.0f, 0.07f, 0.05f);
     omuni.set_pid(2, 3.0f, 0.07f, 0.05f);
     
-    lift_pid.param(0.2, 0.0, 0.0);
+    lift_pid.param(0.4, 0.0, 0.0);
     lift_pid.period(0.001);
     lift_pid.output(-127, 127);