Diff: main.cpp

Revision:

9:bf6d20813364

Parent:

8:956a76b98da0

Child:

10:f17b33dd837d

--- a/main.cpp	Tue Sep 19 03:19:28 2017 +0000
+++ b/main.cpp	Thu Sep 28 10:20:53 2017 +0000
@@ -1,24 +1,22 @@
 #include "mbed.h"
 #include "omuni.hpp"
 #include "cal_PID.hpp"
-#include "timeBaseTrapezoidalMotionCal.h"
 #include "FastPWM.h"
-#include "soundData.h"
+#include "sound_data.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;
-const float omuni_burst_coeff = 1.5f;
-const float omega_max = 0.7 * 2 * 3.14159265f;
-const float omega_f = 0.7 * 2 * 3.14159265f;
+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 = 1.8f;
+const float wrap_speed = 2.2f;
 const int8_t armDuty[] = {-100, -100};
 const signed int spearDuty = 127;
-//const int32_t lift_preset_min = 200;
-const int32_t lift_preset_min = 320;  // Temporary use
+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[4] = {
     lift_preset_min,
@@ -30,7 +28,7 @@
 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_prescaler = 10;
+const int32_t lift_prescaler = 5;
 
 
 const int omuniAddr[] = {0x10, 0x12, 0x14}; // 0000 , 1000 , 0100
@@ -49,6 +47,8 @@
 bool arm = false;
 bool spear = false;
 
+bool check_beep = false;
+
 int lift_targ = lift_preset[0];
 int lift_stepTarg;
 bool lift_stepMoving = false;
@@ -56,9 +56,7 @@
 int past_lift_gray = 0b00;
 int lift_inc = 0;
 
-const float soundGain = 1.0;
-int soundDataCount = 0;
-bool soundEnd = false;
+int sound_count = 0;
 
 DigitalIn button(USER_BUTTON);
 DigitalOut led(LED1);
@@ -66,15 +64,14 @@
 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);
+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;
+FastPWM soundOut(PB_7);
 
 float speed_x, speed_y, omega;
 bool f;
@@ -113,6 +110,22 @@
 int drive_motor(int address,signed int duty);
 void emergencyStop();
 
+/***** sound *****/
+void pi(int times);
+void esc_sound(int key);
+void q_sound();
+void ans_sound();
+void piroro();
+void beep(int T_us,int t_ms);
+void beep_freq(int freq, int t_ms);
+void beep_note(int note, int t_ms);
+void check_beep_turn(bool turn);
+int getPeriod(int freq);
+int freq2period_us(int freq);
+int note2period_us(int note);
+
+/*****************/
+
 comBuf_t comBuf;
 
 void reset(){
@@ -209,8 +222,17 @@
         reset();
     }
     
-    speed_x = omuni_speed_max * getJoy7bit(buf->joyLX) / 64.0f;
-    speed_y = omuni_speed_max * getJoy7bit(buf->joyLY) / 64.0f;
+    
+    if(check_beep == false && buf->SELECT == true){
+        check_beep_turn(true);
+    }
+    else if(check_beep == true && buf->SELECT == false){
+        check_beep_turn(false);
+    }
+    check_beep = buf->SELECT;
+    
+    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;
@@ -220,36 +242,33 @@
     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;
     }
     else{
-        int diff = (int)buf->R2 - (int)buf->L2;
+        int diff = (int)buf->L2 - (int)buf->R2;
         omega = omega_max * diff / 127.0f;
         f = 0; 
     }
     
-    /*
-    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->R2 * 1.0f / 127.0f;
-        f = 0;
-    }
-    else{
-        omega = 0;
-        f = 0;
-    }
-    */
-    
     if(buf->R1 || buf->L1){
-        speed_x = wrap_speed;
-        speed_y = 0;
-        f = 0;
-        omega = wrap_speed / wrap_radius;
-        speed_x *= buf->L1 ? 1 : -1;
-        omega *= buf->L1 ? 1 : -1;
+        float speed_wrap_x;
+        float speed_wrap_y;
+        float omega_wrap;
+        
+        speed_wrap_x = wrap_speed;
+        speed_wrap_y = 0;
+        omega_wrap = wrap_speed / wrap_radius;
+        speed_wrap_x *= buf->R1 ? -1 : 1;
+        omega_wrap *= buf->R1 ? 1 : -1;
+        
+        if(f == 1){
+            speed_x = 0;
+            speed_y = 0;
+            omega = 0;
+        }
+        speed_x += speed_wrap_x;
+        speed_y += speed_wrap_y;
+        omega   += omega_wrap;
     }
     
     arm = buf->arm != 0;
@@ -318,21 +337,6 @@
     
 }
 
-
-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){
@@ -493,9 +497,6 @@
 
 int main()
 {
-    int period_us = 0;
-    int count = 0;
-    
     pc.baud(115200);
     com.baud(115200);
     pc.printf("Hello!\n");
@@ -505,7 +506,6 @@
     
     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);
     
@@ -517,16 +517,13 @@
     
     led = 0;
     
+    //esc_sound(0);
     /*
-    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);
+    beep_note(96, 150);
+    beep_note(98, 150);
+    beep_note(100, 220);
     */
+    beep_note(100, 500);
     
     while(1)
     {
@@ -570,3 +567,119 @@
 }
 
 
+
+/*************** sound ***************/
+
+void pi(int times){
+    int count=0;
+    
+    for(count=0;count<times;count++){
+        beep(379,50);
+        wait_ms(50);
+    }
+    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;
+    
+    wait_ms(60);
+    beep_note(96 + key,150);
+    beep_note(98 + key,150);
+    beep_note(100 + key,220);
+    wait_ms(1200);
+    for(count=0;count<6;count++){
+        beep_note(96 + key,110);
+        wait_ms(150);
+    }
+    wait_ms(1000);
+    beep_note(96 + key,300);
+    wait_ms(100);
+}
+
+void q_sound(void){
+    beep(478,100);
+    beep(379,100);
+}
+
+void ans_sound(void){
+    beep(379,100);
+    beep(478,100);
+}
+
+void piroro(void){
+    beep(379,100);
+    beep(426,100);
+    beep(478,100);
+}
+
+void beep(int T_us,int t_ms){
+    
+    if(T_us==0 || t_ms==0)return;
+    
+    soundOut.period_us(T_us);
+    soundOut.write(0.50);
+    
+    wait_ms(t_ms);
+    
+    soundOut.write(0.0);
+    soundOut.period_us(100);
+    
+    return;
+}
+
+void beep_freq(int freq,int t_ms){
+    beep(1000000.0 / freq, t_ms);
+}
+
+void beep_note(int note, int t_ms){
+    beep(pow(2.0, (69 - note) / 12.0) * 1000000.0 / 440.0, t_ms);
+}
+
+void check_beep_turn(bool turn){
+    if(turn){
+        soundOut.period_us(note2period_us(note_kouka[sound_count] + kouka_key));
+        soundOut.write(0.50);
+        if(sound_count < kouka_size - 1)sound_count++;
+        else sound_count = 0;
+    }
+    else{
+        soundOut.write(0.0);
+    }
+}
+
+int getPeriod_us(int freq){
+    if(freq<=0)return 0;
+    return 1000000.0/freq ;
+}
+
+
+int freq2period_us(int freq){
+    if(freq<=0)return 0;
+    return 1000000.0/freq ;
+}
+
+int note2period_us(int note){
+    return freq2period_us(440.0 * pow(2.0, (note - 69) / 12.0));
+}
+
+
+/*******************************************/
+