Diff: main.cpp

Revision:

10:f17b33dd837d

Parent:

9:bf6d20813364

diff -r bf6d20813364 -r f17b33dd837d main.cpp
--- a/main.cpp	Thu Sep 28 10:20:53 2017 +0000
+++ b/main.cpp	Mon Oct 09 02:29:36 2017 +0000
@@ -7,27 +7,26 @@
 #define LIFT_METER_INVERSION    1
 
 const int comTimeout_ms = 200;
-const float omuni_speed_max = 3.0f;
-const float omuni_burst_coeff = 1.8f;
-const float omega_max = 1.2 * 2 * 3.14159265f;
-const float omega_f = 0.5 * 2 * 3.14159265f;
-const float wrap_radius = 1.0f;
-const float wrap_speed = 2.2f;
+const float omuni_speed_max = 4.5f;
+const float omuni_burst_coeff = 1.5f;
+const float omega_max = 1.5 * 2 * 3.14159265f;
+const float omega_f = 1.0 * 2 * 3.14159265f;
+const float wrap_radius = 1.5f;
+const float wrap_speed = 3.5f;
 const int8_t armDuty[] = {-100, -100};
 const signed int spearDuty = 127;
-const int32_t lift_preset_min = 250;
-//const int32_t lift_preset_min = 320;  // Temporary use
-const int32_t lift_preset_max = 480;
+const int32_t lift_preset_min = 150;
+const int32_t lift_preset_max = 410; // + 260
 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,
+    (int32_t)((lift_preset_max - lift_preset_min) / 3.0 + lift_preset_min),
+    (int32_t)((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 = 10;
-const int32_t lift_down_speed = 10;
+const int32_t lift_up_speed = 50;
+const int32_t lift_down_speed = 50;
 const int32_t lift_prescaler = 5;
 
 
@@ -41,7 +40,7 @@
 
 bool comTimeout_state = true;
 
-bool ems = false;
+volatile bool ems = false;
 bool recovery = false;
 
 bool arm = false;
@@ -62,7 +61,10 @@
 DigitalOut led(LED1);
 
 Serial pc(USBTX, USBRX);
-Serial com(PA_11, PA_12);
+Serial comMain(PA_11, PA_12);
+Serial comSub(PB_6, PA_10);
+
+
 I2C i2cMaster(D14, D15);
 Omuni omuni(&i2cMaster, TIM1, TIM2, TIM3, 533, 2.0f, omuniAddr, 0.4704f, 0.1f);
 AnalogIn lift_meter(PC_0);
@@ -126,7 +128,9 @@
 
 /*****************/
 
-comBuf_t comBuf;
+comBuf_t comMainBuf;
+comBuf_t comSubBuf;
+
 
 void reset(){
     NVIC_SystemReset();
@@ -173,13 +177,13 @@
 }
 
 
-void com_rx()
+void comMain_rx()
 {
     static int byteCount = 0;
-    char temp = com.getc();
+    char temp = comMain.getc();
     
     if(temp == 0b10000000){
-        comBuf.list[0] = temp;
+        comMainBuf.list[0] = temp;
         byteCount = 1;
     }
     else if((temp & 0b10000000) != 0){ // 想定外のデータ
@@ -187,16 +191,50 @@
     }
     else if(byteCount > 0){
         
-        comBuf.list[byteCount] = temp;
+        comMainBuf.list[byteCount] = temp;
         
         if(byteCount < 9)byteCount += 1;
         else{ // データは揃った
             
-            if(checksum_check(&comBuf)){
+            if(checksum_check(&comMainBuf)){
                 led = !led;
                 comTimeout_count = 0;
                 comTimeout_state = false;
-                received_processing(&comBuf);
+                received_processing(&comMainBuf);
+            }
+            else{
+                byteCount = 0;
+            }
+        }
+        
+    }
+    
+}
+
+void comSub_rx()
+{
+    static int byteCount = 0;
+    char temp = comSub.getc();
+    
+    if(temp == 0b10000000){
+        comSubBuf.list[0] = temp;
+        byteCount = 1;
+    }
+    else if((temp & 0b10000000) != 0){ // 想定外のデータ
+        byteCount = 0;
+    }
+    else if(byteCount > 0){
+        
+        comSubBuf.list[byteCount] = temp;
+        
+        if(byteCount < 9)byteCount += 1;
+        else{ // データは揃った
+            
+            if(checksum_check(&comSubBuf)){
+                led = !led;
+                comTimeout_count = 0;
+                comTimeout_state = false;
+                received_processing(&comSubBuf);
             }
             else{
                 byteCount = 0;
@@ -214,6 +252,11 @@
 void received_processing(comBuf_t *buf){
     
     int lift_nearest_num = 0;
+    float speed = 0;
+    float raw_speed_x, raw_speed_y;
+    float degree;
+    int angle_num;
+    const int omuni_angle_split_num = 8;
     
     if(buf->EMO != 0){
         ems = true;
@@ -231,8 +274,22 @@
     }
     check_beep = buf->SELECT;
     
+    raw_speed_x = -1 * omuni_speed_max * getJoy7bit(buf->joyLX) / 64.0f;
+    raw_speed_y = -1 * omuni_speed_max * getJoy7bit(buf->joyLY) / 64.0f;
+    
+    speed = sqrt(raw_speed_x * raw_speed_x + raw_speed_y * raw_speed_y);
+    
+    degree = atan2((double)raw_speed_y, (double)raw_speed_x) * 180.0 / 3.14159265358979;
+    
+    angle_num = (double)degree * omuni_angle_split_num / 360.0 + ((degree < 0)?-0.5:0.5);
+    
+    speed_x = speed * cos(angle_num * 2 * 3.14159265358979 / omuni_angle_split_num);
+    speed_y = speed * sin(angle_num * 2 * 3.14159265358979 / omuni_angle_split_num);
+    
+    /*
     speed_x = -1 * omuni_speed_max * getJoy7bit(buf->joyLX) / 64.0f;
     speed_y = -1 * omuni_speed_max * getJoy7bit(buf->joyLY) / 64.0f;
+    */
     
     if(buf->pushL == 1){
         speed_x *= omuni_burst_coeff;
@@ -352,6 +409,7 @@
         comTimeout_count += 1;
     }
     
+    
 }
 
 
@@ -412,7 +470,7 @@
 
 void spear_control(){
     const int spear_timeout_short = 100;
-    const int spear_timeout_long = 500;
+    const int spear_timeout_long = 200;
     static int spear_state = 0;
     static int spear_timeout_count = 0;
     signed int duty = 0;
@@ -497,10 +555,15 @@
 
 int main()
 {
+    bool ex_comTimeout_state = true;
+    char currentState;
+    
     pc.baud(115200);
-    com.baud(115200);
+    comMain.baud(19200);
+    comSub.baud(115200);
     pc.printf("Hello!\n");
-    com.attach(com_rx, Serial::RxIrq);
+    comMain.attach(comMain_rx, Serial::RxIrq);
+    comSub.attach(comSub_rx, Serial::RxIrq);
     
     i2cMaster.frequency(400000);
     
@@ -509,7 +572,12 @@
     omuni.set_pid(1, 3.0f, 0.07f, 0.05f);
     omuni.set_pid(2, 3.0f, 0.07f, 0.05f);
     
-    lift_pid.param(1.5, 0.0, 0.0);
+    omuni.change_motor_direction(0, false);
+    omuni.change_motor_direction(1, false);
+    omuni.change_motor_direction(2, false);
+    omuni.change_coordinates_direction(true, true);
+    
+    lift_pid.param(1.0, 0.0, 0.0);
     lift_pid.period(0.001);
     lift_pid.output(-127, 127);
     
@@ -525,10 +593,34 @@
     */
     beep_note(100, 500);
     
+    
+    int initial_meter = lift_meter.read_u16() >> 6;
+#if LIFT_METER_INVERSION
+    initial_meter = 1023 - initial_meter;
+#endif
+    
+    lift_targ = initial_meter;
+    
+    lift_stepTarg = lift_preset[0];
+    lift_stepMoving = true;
+    
+    drive_motor(EMO_Addr, 3);
+    
+    
     while(1)
     {
         wait(0.001);
         
+        i2cMaster.read(EMO_Addr, &currentState, 1);
+        if((currentState & 0b00000010) != 0){
+            for(int count = 0; count < 15; count++){
+                beep_note(100, 100);
+                wait_ms(100);
+            }
+            while(ems == false);
+            emergencyStop();
+        }
+        
         if(comTimeout_state == false){
             control();
         }
@@ -536,6 +628,17 @@
             drive_motor(EMO_Addr, 0);
         }
         
+        if(ex_comTimeout_state == false && comTimeout_state == true){
+            beep_note(100, 100);
+            beep_note(96, 100);
+        }
+        else if(ex_comTimeout_state == true && comTimeout_state == false){
+            beep_note(96, 100);
+            beep_note(100, 100);
+        }
+        
+        ex_comTimeout_state = comTimeout_state;
+        
         if(ems){
             emergencyStop();
         }
@@ -562,7 +665,11 @@
     
     drive_motor(EMO_Addr, 0);
     
-    while(1);
+    beep_note(100, 100);
+    beep_note(96, 100);
+    
+    while(1){
+    }
     
 }
 
@@ -579,23 +686,6 @@
     }
     wait_ms(300);
 }
-/*
-void esc_sound(void){
-    int count=0;
-    
-    wait_ms(60);
-    beep_note(96,150);
-    beep_note(98,150);
-    beep_note(100,220);
-    wait_ms(1200);
-    for(count=0;count<6;count++){
-        beep_note(96,110);
-        wait_ms(150);
-    }
-    wait_ms(1000);
-    beep_note(96,300);
-    wait_ms(100);
-}*/
 
 void esc_sound(int key){
     int count=0;