Diff: main.cpp

Revision:

4:5591d3c8a761

Parent:

3:6223efea43fe

Child:

5:12be2ac0f395

diff -r 6223efea43fe -r 5591d3c8a761 main.cpp
--- a/main.cpp	Mon Aug 28 07:27:23 2017 +0000
+++ b/main.cpp	Mon Sep 11 02:44:27 2017 +0000
@@ -2,15 +2,18 @@
 #include "omuni.hpp"
 #include "cal_PID.hpp"
 #include "timeBaseTrapezoidalMotionCal.h"
+#include "FastPWM.h"
+#include "soundData.h"
 
-const float omuni_speed[4] = {0, 1.0, 1.7, 3.0};
+const int comTimeout_ms = 200;
+const float omuni_speed[4] = {0, 1.0, 2.0, 3.0};
 const float omega_max = 0.7 * 2 * 3.14159265f;
 const float wrap_radius = 1.0f;
-const float wrap_speed = 1.7f;
+const float wrap_speed = 1.8f;
 const int8_t armDuty[] = {-50, -50};
 const signed int spearDuty = 127;
-const int32_t lift_preset_min = 420;
-const int32_t lift_preset_max = 1800;
+const int32_t lift_preset_min = 135;
+const int32_t lift_preset_max = 450;
 const int32_t lift_preset[4] = {
     lift_preset_min,
     (lift_preset_max - lift_preset_min) / 3.0 + lift_preset_min,
@@ -19,14 +22,17 @@
     }; // 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 int32_t lift_up_speed = 5;
+const int32_t lift_down_speed = 5;
 
 
 const int omuniAddr[] = {0x10, 0x12, 0x14}; // 0000 , 1000 , 0100
 const int armAddr[] = {0x16, 0x18};    // 1100 , 0010
 const int spearAddr = 0x1a;            // 1010
 const int liftAddr = 0x1c;             // 0110
+const int EMO_Addr = 0x1e;             // 1110
+
+int comTimeout_count = 0;
 
 int drive_motor(int address,signed int duty);
 void emergencyStop();
@@ -43,18 +49,25 @@
 int past_lift_gray = 0b00;
 int lift_inc = 0;
 
+const float soundGain = 1.0;
+int soundDataCount = 0;
+bool soundEnd = false;
+
 //DigitalIn button(USER_BUTTON);
 DigitalOut led(LED1);
 
-Serial p(USBTX, USBRX);
-Serial pc(PA_11, PA_12);
+Serial pc(USBTX, USBRX);
+Serial com(PA_11, PA_12);
 I2C i2cMaster(D14, D15);
 omuni omuni(&i2cMaster, TIM1, TIM2, TIM3, 533, 2.0f, omuniAddr, 0.4704f, 0.1f);
 AnalogIn lift_meter(PC_0);
 DigitalIn spear_sensor(PC_3);
 Timer mesPeriod;
 cal_pid lift_pid;
+Ticker comTimeout;
 
+FastPWM soundOut(PB_6);
+Ticker soundRenew;
 
 float speed_x, speed_y, omega;
 bool f;
@@ -139,9 +152,9 @@
 }
 
 
-void pc_rx()
+void com_rx()
 {
-    char temp = pc.getc();
+    char temp = com.getc();
     comData_t rcTemp;
     rcTemp.data = temp;
     float rad, speed;
@@ -151,30 +164,31 @@
     RCData[rcTemp.byteNum] = rcTemp;
     
     /*
-    p.printf("%d",rcTemp.bit7);
-    p.printf("%d",rcTemp.bit6);
-    p.printf("%d",rcTemp.bit5);
-    p.printf("%d",rcTemp.bit4);
-    p.printf("%d",rcTemp.bit3);
-    p.printf("%d",rcTemp.bit2);
-    p.printf("%d",rcTemp.bit1);
-    p.printf("%d",rcTemp.bit0);
-    p.printf("\n");
+    pc.printf("%d",rcTemp.bit7);
+    pc.printf("%d",rcTemp.bit6);
+    pc.printf("%d",rcTemp.bit5);
+    pc.printf("%d",rcTemp.bit4);
+    pc.printf("%d",rcTemp.bit3);
+    pc.printf("%d",rcTemp.bit2);
+    pc.printf("%d",rcTemp.bit1);
+    pc.printf("%d",rcTemp.bit0);
+    pc.printf("\n");
     */
-    //p.printf("%d\n", rcTemp.byteNum);
-    //p.printf("speed = %d, angle = %d\n",rcTemp.speed, rcTemp.angle);
+    //pc.printf("%d\n", rcTemp.byteNum);
+    //pc.printf("speed = %d, angle = %d\n",rcTemp.speed, rcTemp.angle);
     
     switch(temp >> 6){
         case 0:
-            //p.printf("speed = %d, angle = %d\n",rcTemp.speed, rcTemp.angle);
+            //pc.printf("speed = %d, angle = %d\n",rcTemp.speed, rcTemp.angle);
             break;
         case 1:
-            //p.printf("dir = %d, omega = %d, coorSys = %d\n", rcTemp.dir, rcTemp.omega, rcTemp.coorSys);
+            //pc.printf("dir = %d, omega = %d, coorSys = %d\n", rcTemp.dir, rcTemp.omega, rcTemp.coorSys);
             break;
         case 2:
-            //p.printf("arm = %d, spear = %d, lift_gray = %d, lift_up = %d, lift_down = %d\n",rcTemp.arm, rcTemp.spear, rcTemp.lift_gray, rcTemp.lift_up, rcTemp.lift_down);
+            //pc.printf("arm = %d, spear = %d, lift_gray = %d, lift_up = %d, lift_down = %d\n",rcTemp.arm, rcTemp.spear, rcTemp.lift_gray, rcTemp.lift_up, rcTemp.lift_down);
             break;
         case 3:
+        
             
             break;
     }
@@ -182,6 +196,7 @@
     if(rcTemp.byteNum == 3){ // データは揃った
         
         led = !led;
+        comTimeout_count = 0;
         
         ems = RCData[3].emo != 0;
         
@@ -237,7 +252,7 @@
             }
             if(lift_diff != 0){
                 if(lift_diff == 1){
-                    if(lift_preset[lift_nearest_num] >= lift_currentTarg){p.printf("$$");
+                    if(lift_preset[lift_nearest_num] >= lift_currentTarg){
                         if(lift_nearest_num < 3)lift_nextTarg = lift_preset[lift_nearest_num + 1];
                     }
                     else{
@@ -261,8 +276,8 @@
         }
         past_lift_gray = RCData[2].lift_gray;
         
-        if(RCData[2].lift_down != 0)lift_inc = -2;
-        else if(RCData[2].lift_up != 0)lift_inc = 2;
+        if(RCData[2].lift_down != 0)lift_inc = -1;
+        else if(RCData[2].lift_up != 0)lift_inc = 1;
         else lift_inc = 0;
         
     } // すっきりした // してない
@@ -270,6 +285,39 @@
 }
 
 
+void soundRenew_intr(){
+    
+    if(soundDataCount < soundData_size - 1000){
+        
+        soundOut.write((soundData[soundDataCount] * soundGain / 256.0) + 0.5);
+        
+        soundDataCount++;
+        
+    }
+    else{
+        soundEnd = true;
+    }
+    
+}
+
+
+void comTimeout_intr(){
+    
+    if(comTimeout_count >= comTimeout_ms){
+        speed_x = 0;
+        speed_y = 0;
+        omega = 0;
+        arm = 0;
+        spear = 0;
+        lift_inc = 0;
+    }
+    else{
+        comTimeout_count += 1;
+    }
+    
+}
+
+
 void arm_control(){
     char armData[2] = {0};
     
@@ -308,14 +356,17 @@
         }
     }
     
-    meter = lift_meter.read_u16() >> 4;
+    meter = lift_meter.read_u16() >> 6;
     duty = lift_pid.get_pid(meter, lift_targ, -1);
     drive_motor(liftAddr, duty);
     
 }
 
 void spear_control(){
+    const int spear_timeout_short = 100;
+    const int spear_timeout_long = 500;
     static int spear_state = 0;
+    static int spear_timeout_count = 0;
     signed int duty = 0;
     int sensor = spear_sensor;  // リミット時に1
     
@@ -332,15 +383,33 @@
             break;
         case 1 : // 初期位置から抜け出す
             duty = spearDuty;
-            if(sensor == 0){
+            if(sensor != 0){
+                if(spear_timeout_count < spear_timeout_short){
+                    spear_timeout_count += 1;
+                }
+                else{
+                    spear_timeout_count = 0;
+                    spear_state = 0;
+                }
+            }
+            else{
+                spear_timeout_count = 0;
                 spear_state = 2;
             }
             break;
         case 2 : // 到達位置まで動かす
             if(sensor == 0){
                 duty = spearDuty;
+                if(spear_timeout_count < spear_timeout_long){
+                    spear_timeout_count += 1;
+                }
+                else{
+                    spear_timeout_count = 0;
+                    spear_state = 3;
+                }
             }
             else{
+                spear_timeout_count = 0;
                 spear_state = 3;
             }
             break;
@@ -378,27 +447,37 @@
     int period_us = 0;
     int count = 0;
     
-    p.baud(115200);
-    pc.baud(9600);
-    p.printf("Hello!\n");
-    pc.attach(pc_rx, Serial::RxIrq);
+    pc.baud(115200);
+    com.baud(9600);
+    pc.printf("Hello!\n");
+    com.attach(com_rx, Serial::RxIrq);
     
     i2cMaster.frequency(400000);
     
     omuni.set_speed(0.0f, 0.0f);
     omuni.set_pid(0, 3.0f, 0.07f, 0.05f);
+    //omuni.set_pid(0, 6.0f, 0.14f, 0.10f);
     omuni.set_pid(1, 3.0f, 0.07f, 0.05f);
     omuni.set_pid(2, 3.0f, 0.07f, 0.05f);
     
-    lift_pid.param(0.4, 0.0, 0.0);
+    lift_pid.param(1.5, 0.0, 0.0);
     lift_pid.period(0.001);
     lift_pid.output(-127, 127);
     
-    //control_loop.attach_us(&control, 1000);
+    comTimeout.attach_us(comTimeout_intr, 1000);
     
     led = 0;
     
-    //mesPeriod.start();
+    /*
+    soundOut.period_us(10);
+    for(count = 0; count < 500; count++){
+        wait_ms(1);
+        soundOut.write(count / 100.0);
+    }
+    soundRenew.attach_us(soundRenew_intr, 63);
+    
+    while(soundEnd == false);
+    */
     
     while(1)
     {
@@ -407,6 +486,8 @@
         omuni.drive();
         control();
         
+        drive_motor(EMO_Addr, 1);
+        
         if(ems){
             emergencyStop();
         }
@@ -422,7 +503,6 @@
     return ack;
 }
 
-
 void emergencyStop(){
     drive_motor(omuniAddr[0], 0);
     drive_motor(omuniAddr[1], 0);
@@ -431,6 +511,9 @@
     drive_motor(armAddr[1], 0);
     drive_motor(liftAddr, 0);
     drive_motor(spearAddr, 0);
+    
+    drive_motor(EMO_Addr, 0);
+    
     while(1);
 }