File content as of revision 1:d6e30aa7bc5b:

#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 ;
}



/*******************************************/