Diff: main.cpp

Revision:

2:c9de02d6d154

Parent:

1:baab5b88a142

Child:

3:6223efea43fe

diff -r baab5b88a142 -r c9de02d6d154 main.cpp
--- a/main.cpp	Wed Aug 23 07:50:14 2017 +0000
+++ b/main.cpp	Mon Aug 28 01:39:14 2017 +0000
@@ -1,15 +1,16 @@
 #include "mbed.h"
 #include "omuni.hpp"
 #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 int8_t armDuty[] = {-50, -50};
 const signed int spearDuty = 127;
-const int32_t lift_preset[4] = {620, 990, 1360, 1730}; // 12bit 0 ~ 4095
-const int32_t lift_max = 1830;
-const int32_t lift_min = 500;
+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 int omuniAddr[] = {0x10, 0x12, 0x14}; // 0000 , 1000 , 0100
 const int armAddr[] = {0x16, 0x18};    // 1100 , 0010
@@ -23,10 +24,15 @@
 
 bool arm = false;
 bool spear = false;
+
 int lift_targ = lift_preset[0];
+int lift_stepTarg;
+int lift_stepOrigin;
 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);
@@ -39,6 +45,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;
@@ -165,7 +172,7 @@
     
     if(rcTemp.byteNum == 3){ // データは揃った
         
-        led = !led;
+//        led = !led;
         
         ems = RCData[3].emo != 0;
         
@@ -198,45 +205,51 @@
             first_receive = false;
         }
         if(RCData[2].lift_gray != past_lift_gray){ // 段階移動
+            int lift_currentTarg;
             int lift_diff = 0;
             int lift_nearest = 10000;
+            int lift_nextTarg;
+            
+            lift_currentTarg = lift_targ;
+            lift_nextTarg = lift_currentTarg;
+            
             lift_diff = grayDiffer2bit(RCData[2].lift_gray, past_lift_gray);
-            //p.printf("%d --> %d\n", past_lift_gray, RCData[2].lift_gray);
-            //p.printf("gray = %d\n", lift_diff);
             for(int num = 0; num < 4; num++){ // 最寄りの段階
-                //p.printf("nearest = %d, num = %d\n", lift_nearest, lift_nearest_num);
-                if(lift_nearest > abs(lift_preset[num] - lift_targ)){
+                if(lift_nearest > abs(lift_preset[num] - lift_currentTarg)){
                     lift_nearest_num = num;
-                    lift_nearest = abs(lift_preset[num] - lift_targ);
+                    lift_nearest = abs(lift_preset[num] - lift_currentTarg);
                 }
             }
-            //p.printf("lift_targ = %d num = %d\n", lift_targ, lift_nearest_num);
             if(lift_diff == 1){
-                if(lift_preset[lift_nearest_num] >= lift_targ){
-                    if(lift_nearest_num < 3)lift_targ = lift_preset[lift_nearest_num + 1];
+                if(lift_preset[lift_nearest_num] >= lift_currentTarg){
+                    if(lift_nearest_num < 3)lift_nextTarg = lift_preset[lift_nearest_num + 1];
                 }
                 else{
-                    lift_targ = lift_preset[lift_nearest_num];
+                    lift_nextTarg = lift_preset[lift_nearest_num];
                 }
             }
             else if(lift_diff == -1){
-                if(lift_preset[lift_nearest_num] <= lift_targ){
-                    if(lift_nearest_num > 0)lift_targ = lift_preset[lift_nearest_num - 1];
+                if(lift_preset[lift_nearest_num] <= lift_currentTarg){
+                    if(lift_nearest_num > 0)lift_nextTarg = lift_preset[lift_nearest_num - 1];
                 }
                 else{
-                    lift_targ = lift_preset[lift_nearest_num];
+                    lift_nextTarg = lift_preset[lift_nearest_num];
                 }
             }
+            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;
         
-        if(RCData[2].lift_down != 0)lift_inc = -5;
-        else if(RCData[2].lift_up != 0)lift_inc = 5;
+        if(RCData[2].lift_down != 0)lift_inc = -2;
+        else if(RCData[2].lift_up != 0)lift_inc = 2;
         else lift_inc = 0;
         
-        //p.printf("inc = %d\n",lift_inc);
-        //p.printf("lift_targ = %d\n", lift_targ);
-        
     } // すっきりした // してない
     
 }
@@ -257,12 +270,24 @@
     int32_t meter = 0;
     signed int duty = 0;
     
-    if(lift_inc < 0){
-        if(lift_min < lift_targ + lift_inc)lift_targ += lift_inc;
+    if(lift_task != 0){
+        if(lift_targ != lift_stepTarg){
+            lift_targ = lift_motionCal.calSteps(lift_time) + lift_stepOrigin;
+            lift_time += 1;
+        }
+        else{
+            lift_task = 0;
+        }
     }
-    else if(lift_inc > 0){
-        if(lift_max > lift_targ + lift_inc)lift_targ += lift_inc;
+    else{
+        if(lift_inc < 0){
+            if(lift_min < lift_targ + lift_inc)lift_targ += lift_inc;
+        }
+        else if(lift_inc > 0){
+            if(lift_max > lift_targ + lift_inc)lift_targ += lift_inc;
+        }
     }
+    
     meter = lift_meter.read_u16() >> 4;
     duty = lift_pid.get_pid(meter, lift_targ);
     drive_motor(liftAddr, duty);
@@ -345,7 +370,7 @@
     omuni.set_pid(1, 3.0f, 0.07f, 0.05f);
     omuni.set_pid(2, 3.0f, 0.07f, 0.05f);
     
-    lift_pid.param(0.3, 0.0, 0.0);
+    lift_pid.param(0.2, 0.0, 0.0);
     lift_pid.period(0.001);
     lift_pid.output(-127, 127);
     
@@ -357,7 +382,7 @@
     
     while(1)
     {
-        wait(0.0001);
+        wait(0.001);
         omuni.set_speed(speed_x, speed_y, omega, f);
         omuni.drive();
         control();
@@ -365,19 +390,6 @@
         if(ems){
             emergencyStop();
         }
-        /*
-        if(count < 100){
-            count += 1;
-        }
-        else{
-            period_us = mesPeriod.read_us();
-            mesPeriod.reset();
-            
-            p.printf("%dHz\n", 1000000 * 100 / period_us);
-            
-            count = 0;
-        }
-        */
     }
 }