Diff: main.cpp

Revision:

0:4346c5764e83

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Sep 25 08:16:54 2017 +0000
@@ -0,0 +1,379 @@
+#include "mbed.h"
+#include "omuni.hpp"
+#include "cal_PID.hpp"
+#include "FastPWM.h"
+
+const int comTimeout_ms = 200;
+const float omuni_speed_max = 2.3f;
+const float omuni_burst_coeff = 1.7f;
+const float omega_max = 1.0 * 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 int8_t armDuty[] = {-100, -100};
+
+const int omuniAddr[] = {0x10, 0x12, 0x14}; // 0000 , 1000 , 0100
+const int armAddr[] = {0x16, 0x18};    // 1100 , 0010
+const int EMO_Addr = 0x1e;             // 1110
+
+int comTimeout_count = 0;
+
+bool comTimeout_state = true;
+
+bool ems = false;
+bool recovery = false;
+
+bool arm = false;
+
+DigitalIn button(USER_BUTTON);
+DigitalOut led(LED1);
+
+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);
+Ticker comTimeout;
+
+FastPWM soundOut(PB_7);
+
+float speed_x, speed_y, omega;
+bool f;
+
+typedef union{
+    struct{
+        unsigned startByte :8;
+        unsigned joyLX     :8;
+        unsigned joyLY     :8;
+        unsigned joyRX     :8;
+        unsigned joyRY     :8;
+        unsigned L2        :8;
+        unsigned R2        :8;
+        unsigned arrowL    :1;
+        unsigned arrowR    :1;
+        unsigned arrowU    :1;
+        unsigned arrowD    :1;
+        unsigned L1        :1;
+        unsigned R1        :1;
+        unsigned SELECT    :2;
+        unsigned lift_gray :2;
+        unsigned spear     :1;
+        unsigned arm       :1;
+        unsigned pushL     :1;
+        unsigned pushR     :1;
+        unsigned EMO       :2;
+        unsigned checksum  :8;
+    };
+    
+    uint8_t list[10];
+}comBuf_t;
+
+void received_processing(comBuf_t *buf);
+int byteSum(int8_t byte);
+bool checksum_check(comBuf_t *buf);
+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);
+int getPeriod(int freq);
+/*****************/
+
+comBuf_t comBuf;
+
+void reset(){
+    NVIC_SystemReset();
+}
+
+bool checksum_check(comBuf_t *buf){
+    int sum = 0;
+    
+    for(int count = 0; count < 9; count++){
+        sum += buf->list[count];
+    }
+    return (sum & 0b01111111) == buf->checksum;
+}
+
+
+void com_rx()
+{
+    static int byteCount = 0;
+    char temp = com.getc();
+    
+    if(temp == 0b10000000){
+        comBuf.list[0] = temp;
+        byteCount = 1;
+    }
+    else if((temp & 0b10000000) != 0){ // 想定外のデータ
+        byteCount = 0;
+    }
+    else if(byteCount > 0){
+        
+        comBuf.list[byteCount] = temp;
+        
+        if(byteCount < 9)byteCount += 1;
+        else{ // データは揃った
+            
+            if(checksum_check(&comBuf)){
+                led = !led;
+                comTimeout_count = 0;
+                comTimeout_state = false;
+                received_processing(&comBuf);
+            }
+            else{
+                byteCount = 0;
+            }
+        }
+        
+    }
+    
+}
+
+int getJoy7bit(int raw){
+    return raw - 64;
+}
+
+void received_processing(comBuf_t *buf){
+    
+    if(buf->EMO != 0){
+        ems = true;
+    }
+    else if(ems == true){
+        reset();
+    }
+    
+    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;
+        speed_y *= omuni_burst_coeff;
+    }
+    
+    if(abs(getJoy7bit(buf->joyRY)) < 20 && abs(getJoy7bit(buf->joyRX)) > 50){
+        f = 1;
+        omega = omega_f;
+        if(getJoy7bit(buf->joyRX) > 0)omega *= -1;
+    }
+    else{
+        int diff = (int)buf->L2 - (int)buf->R2;
+        omega = omega_max * diff / 127.0f;
+        f = 0; 
+    }
+    
+    if(buf->R1 || buf->L1){
+        speed_x = wrap_speed;
+        speed_y = 0;
+        f = 0;
+        omega = wrap_speed / wrap_radius;
+        speed_x *= buf->R1 ? -1 : 1;
+        omega *= buf->R1 ? 1 : -1;
+    }
+    
+    arm = buf->arm != 0;
+    
+}
+
+void comTimeout_intr(){
+    
+    if(comTimeout_count >= comTimeout_ms){
+        speed_x = 0;
+        speed_y = 0;
+        omega = 0;
+        arm = 0;
+        comTimeout_state = true;
+    }
+    else{
+        comTimeout_count += 1;
+    }
+    
+}
+
+
+void arm_control(){
+    char armData[2] = {0};
+    
+    armData[0] = arm? armDuty[0] : 0 ;
+    armData[1] = arm? armDuty[1] : 0 ;
+    
+    i2cMaster.write(armAddr[0], &armData[0], 1);
+    i2cMaster.write(armAddr[1], &armData[1], 1);
+    
+}
+
+void control(){
+    
+    drive_motor(EMO_Addr, 1);
+    
+    omuni.set_speed(speed_x, speed_y, omega, f);
+    omuni.drive();
+    
+    arm_control();
+    
+}
+
+int main()
+{
+    
+    pc.baud(115200);
+    com.baud(115200);
+    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);
+    
+    comTimeout.attach_us(comTimeout_intr, 1000);
+    
+    led = 0;
+    
+    //esc_sound(0);
+    beep_note(96, 100);
+    
+    while(1)
+    {
+        wait(0.001);
+        
+        if(comTimeout_state == false){
+            control();
+        }
+        else{
+            drive_motor(EMO_Addr, 0);
+        }
+        
+        if(ems){
+            emergencyStop();
+        }
+    }
+}
+
+int drive_motor(int address,signed int duty){ /* アドレスを指定してモータを駆動 */
+    char send_data;
+    int ack;
+    if((duty>127)||(duty<-128))return -1; /* 範囲外なら送信しない */
+    send_data=duty;
+    ack=i2cMaster.write(address,&send_data,1);
+    return ack;
+}
+
+void emergencyStop(){
+    drive_motor(omuniAddr[0], 0);
+    drive_motor(omuniAddr[1], 0);
+    drive_motor(omuniAddr[2], 0);
+    drive_motor(armAddr[0], 0);
+    drive_motor(armAddr[1], 0);
+    
+    drive_motor(EMO_Addr, 0);
+    
+    while(1);
+    
+}
+
+
+
+/*************** 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.10);
+    
+    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);
+}
+
+int getPeriod_us(int freq){
+    if(freq<=0)return 0;
+    return 1000000.0/freq ;
+}
+
+
+
+/*******************************************/
+