Diff: main.cpp

Revision:

6:22ad12682a5d

Parent:

5:12be2ac0f395

Child:

7:5cdd805ca8a3

--- a/main.cpp	Mon Sep 11 05:37:59 2017 +0000
+++ b/main.cpp	Mon Sep 18 11:51:23 2017 +0000
@@ -5,6 +5,8 @@
 #include "FastPWM.h"
 #include "soundData.h"
 
+#define LIFT_METER_INVERSION    1
+
 const int comTimeout_ms = 200;
 const float omuni_speed[4] = {0, 1.0, 2.0, 3.0};
 const float omuni_speed_max = 2.0f; 
@@ -12,10 +14,11 @@
 const float omega_f = 0.7 * 2 * 3.14159265f;
 const float wrap_radius = 1.0f;
 const float wrap_speed = 1.8f;
-const int8_t armDuty[] = {-50, -50};
+const int8_t armDuty[] = {-100, -100};
 const signed int spearDuty = 127;
-const int32_t lift_preset_min = 135;
-const int32_t lift_preset_max = 450;
+//const int32_t lift_preset_min = 200;
+const int32_t lift_preset_min = 320;  // Temporary use
+const int32_t lift_preset_max = 480;
 const int32_t lift_preset[4] = {
     lift_preset_min,
     (lift_preset_max - lift_preset_min) / 3.0 + lift_preset_min,
@@ -24,8 +27,8 @@
     }; // 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 = 5;
-const int32_t lift_down_speed = 5;
+const int32_t lift_up_speed = 2;
+const int32_t lift_down_speed = 2;
 
 
 const int omuniAddr[] = {0x10, 0x12, 0x14}; // 0000 , 1000 , 0100
@@ -36,7 +39,7 @@
 
 int comTimeout_count = 0;
 
-
+bool comTimeout_state = false;
 
 bool ems = false;
 
@@ -54,7 +57,7 @@
 int soundDataCount = 0;
 bool soundEnd = false;
 
-//DigitalIn button(USER_BUTTON);
+DigitalIn button(USER_BUTTON);
 DigitalOut led(LED1);
 
 Serial pc(USBTX, USBRX);
@@ -141,21 +144,13 @@
     return 0;
 }
 
-int byteSum(int8_t byte){
-    int sum = 0;
-    for(int count = 0; count < 8; count++){
-        if((byte & (0x01 << count)) != 0)sum += 1;
-    }
-    return sum;
-}
-
 bool checksum_check(comBuf_t *buf){
     int sum = 0;
     
     for(int count = 0; count < 9; count++){
-        sum += byteSum(buf->list[count]);
+        sum += buf->list[count];
     }
-    return sum == buf->checksum;
+    return (sum & 0b01111111) == buf->checksum;
 }
 
 
@@ -168,7 +163,7 @@
         comBuf.list[0] = temp;
         byteCount = 1;
     }
-    else if(byteCount & 0b10000000 != 0){ // 想定外のデータ
+    else if((temp & 0b10000000) != 0){ // 想定外のデータ
         byteCount = 0;
     }
     else if(byteCount > 0){
@@ -177,6 +172,7 @@
         
         if(byteCount < 9)byteCount += 1;
         else{ // データは揃った
+            
             if(checksum_check(&comBuf)){
                 led = !led;
                 comTimeout_count = 0;
@@ -204,18 +200,21 @@
     speed_x = omuni_speed_max * getJoy7bit(buf->joyLX) / 64.0f;
     speed_y = omuni_speed_max * getJoy7bit(buf->joyLY) / 64.0f;
     
-    if(abs(getJoy7bit(buf->joyRY)) < 10 && abs(getJoy7bit(buf->joyRX)) > 40){
+    if(abs(getJoy7bit(buf->joyRY)) < 20 && abs(getJoy7bit(buf->joyRX)) > 50){
         f = 1;
         omega = omega_f;
-        if(getJoy7bit(buf->joyRX) < 0)omega *= 1;
+        if(getJoy7bit(buf->joyRX) < 0)omega *= -1;
     }
-    
-    if(buf->L2 >= 5 && buf->R2 <  5){
-        omega = omega_max * buf->L2 / 127.0f;
+    else if(buf->L2 >= 5 && buf->R2 <  5){
+        omega = omega_max * buf->L2 * -1.0f / 127.0f;
         f = 0;
     }
     else if(buf->L2 <  5 && buf->R2 >= 5){
-        omega = omega_max * buf->L2  * -1.0f / 127.0f;
+        omega = omega_max * buf->R2 * 1.0f / 127.0f;
+        f = 0;
+    }
+    else{
+        omega = 0;
         f = 0;
     }
     
@@ -283,7 +282,7 @@
     }
     past_lift_gray = buf->lift_gray;
     
-    if(abs(getJoy7bit(buf->joyRY)) > 40 && abs(getJoy7bit(buf->joyRX)) < 10){
+    if(abs(getJoy7bit(buf->joyRY)) > 50 && abs(getJoy7bit(buf->joyRX)) < 20){
         if(getJoy7bit(buf->joyRY) > 0)lift_inc = 1;
         else lift_inc = -1;
     }
@@ -319,9 +318,11 @@
         arm = 0;
         spear = 0;
         lift_inc = 0;
+        comTimeout_state = true;
     }
     else{
         comTimeout_count += 1;
+        comTimeout_state = false;
     }
     
 }
@@ -366,7 +367,10 @@
     }
     
     meter = lift_meter.read_u16() >> 6;
-    duty = lift_pid.get_pid(meter, lift_targ, -1);
+#if LIFT_METER_INVERSION
+    meter = 1023 - meter;
+#endif
+    duty = lift_pid.get_pid(meter, lift_targ, 1);
     drive_motor(liftAddr, duty);
     
 }
@@ -445,6 +449,11 @@
 
 void control(){
     
+    drive_motor(EMO_Addr, 1);
+    
+    omuni.set_speed(speed_x, speed_y, omega, f);
+    omuni.drive();
+    
     arm_control();
     lift_control();
     spear_control();
@@ -457,7 +466,7 @@
     int count = 0;
     
     pc.baud(115200);
-    com.baud(9600);
+    com.baud(115200);
     pc.printf("Hello!\n");
     com.attach(com_rx, Serial::RxIrq);
     
@@ -491,11 +500,13 @@
     while(1)
     {
         wait(0.001);
-        omuni.set_speed(speed_x, speed_y, omega, f);
-        omuni.drive();
-        control();
         
-        drive_motor(EMO_Addr, 1);
+        if(comTimeout_state == false){
+            control();
+        }
+        else{
+            drive_motor(EMO_Addr, 0);
+        }
         
         if(ems){
             emergencyStop();