File content as of revision 11:c7b0c5f8fd22:

#include "mbed.h"
#include "omuni.hpp"
#include "cal_PID.hpp"
#include "FastPWM.h"
#include "sound_data.h"

#define LIFT_METER_INVERSION    1

const int comTimeout_ms = 200;
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 = 150;
const int32_t lift_preset_max = 410; // + 260
const int32_t lift_preset[4] = {
    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 = 50;
const int32_t lift_down_speed = 50;
const int32_t lift_prescaler = 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;

bool comTimeout_state = true;

volatile bool ems = false;
bool recovery = false;

bool arm = false;
bool spear = false;

bool check_beep = false;

int lift_targ = lift_preset[0];
int lift_stepTarg;
bool lift_stepMoving = false;
bool first_receive = true;
int past_lift_gray = 0b00;
int lift_inc = 0;

int sound_count = 0;

DigitalIn button(USER_BUTTON);
DigitalOut led(LED1);

Serial pc(USBTX, USBRX);
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);
DigitalIn spear_sensor(PC_3);
Timer mesPeriod;
cal_pid lift_pid;
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);
void check_beep_turn(bool turn);
int getPeriod(int freq);
int freq2period_us(int freq);
int note2period_us(int note);

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

comBuf_t comMainBuf;
comBuf_t comSubBuf;


void reset(){
    NVIC_SystemReset();
}

int grayDiffer2bit(int gray, int origin){
    
    switch(origin){
        case 0b00:
            switch(gray){
                case 0b01: return 1;
                case 0b10: return -1;
                default  : return 0;
            }
        case 0b01:
            switch(gray){
                case 0b11: return 1;
                case 0b00: return -1;
                default  : return 0;
            }
        case 0b11:
            switch(gray){
                case 0b10: return 1;
                case 0b01: return -1;
                default  : return 0;
            }
        case 0b10:
            switch(gray){
                case 0b00: return 1;
                case 0b11: return -1;
                default  : return 0;
            }
    }
    return 0;
}

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 comMain_rx()
{
    static int byteCount = 0;
    char temp = comMain.getc();
    
    if(temp == 0b10000000){
        comMainBuf.list[0] = temp;
        byteCount = 1;
    }
    else if((temp & 0b10000000) != 0){ // 想定外のデータ
        byteCount = 0;
    }
    else if(byteCount > 0){
        
        comMainBuf.list[byteCount] = temp;
        
        if(byteCount < 9)byteCount += 1;
        else{ // データは揃った
            
            if(checksum_check(&comMainBuf)){
                led = !led;
                comTimeout_count = 0;
                comTimeout_state = false;
                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;
            }
        }
        
    }
    
}

int getJoy7bit(int raw){
    return raw - 64;
}

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;
    }
    else if(ems == true){
        reset();
    }
    
    
    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;
    
    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;
        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){
        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;
    
    spear = buf->spear != 0;      
    
    if(first_receive){
        past_lift_gray = buf->lift_gray;
        first_receive = false;
    }
    if(buf->lift_gray != past_lift_gray){ // 段階移動
        int lift_currentTarg;
        int lift_diff = 0;
        int lift_nearest = 10000;
        int lift_nextTarg;
        
        lift_currentTarg = lift_targ;
        if(lift_stepMoving){
            lift_nextTarg = lift_stepTarg;
        }
        else{
            lift_nextTarg = lift_currentTarg;
        }
        
        lift_diff = grayDiffer2bit(buf->lift_gray, past_lift_gray);
        for(int num = 0; num < 4; num++){ // 最寄りの段階
            if(lift_nearest > abs(lift_preset[num] - lift_currentTarg)){
                lift_nearest_num = num;
                lift_nearest = abs(lift_preset[num] - lift_currentTarg);
            }
        }
        if(lift_diff != 0){
            if(lift_diff == 1){
                if(lift_preset[lift_nearest_num] <= lift_currentTarg){
                    if(lift_nearest_num < 3)lift_nextTarg = lift_preset[lift_nearest_num + 1];
                }
                else{
                    lift_nextTarg = lift_preset[lift_nearest_num];
                }
            }
            else if(lift_diff == -1){
                if(lift_preset[lift_nearest_num] >= lift_currentTarg){
                    if(lift_nearest_num > 0)lift_nextTarg = lift_preset[lift_nearest_num - 1];
                }
                else{
                    lift_nextTarg = lift_preset[lift_nearest_num];
                }
            }
            
            lift_stepTarg = lift_nextTarg;
            lift_stepMoving = true;
                            
        }
        
    }
    past_lift_gray = buf->lift_gray;
    
    
    if(abs(getJoy7bit(buf->joyRY)) > 50 && abs(getJoy7bit(buf->joyRX)) < 20){
        if(getJoy7bit(buf->joyRY) > 0)lift_inc = -5;
        else lift_inc = 5;
    }
    else{
        lift_inc = 0;
    }
    
}

void comTimeout_intr(){
    
    if(comTimeout_count >= comTimeout_ms){
        speed_x = 0;
        speed_y = 0;
        omega = 0;
        arm = 0;
        spear = 0;
        lift_inc = 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 lift_control(){
    static int prescaler_count = 0;
    int32_t meter = 0;
    signed int duty = 0;
    int32_t error = 0;
    
    if(prescaler_count >= lift_prescaler){
        prescaler_count = 0;
        
        error = lift_stepTarg - lift_targ;
        if(lift_stepMoving){
            if(error > lift_up_speed){
                lift_targ += lift_up_speed; 
            }
            else if(error < lift_down_speed * -1){
                lift_targ -= lift_down_speed;
            }
            else{
                lift_targ = lift_stepTarg;
                lift_stepMoving = false;
            }
        }
        else{
            if(lift_inc < 0){
                if(lift_min < lift_targ + lift_inc)lift_targ += lift_inc;
            }
            else if(lift_inc > 0){
                if(lift_max > lift_targ + lift_inc)lift_targ += lift_inc;
            }
        }
    }
    else{
        prescaler_count += 1;
    }
    
    meter = lift_meter.read_u16() >> 6;
#if LIFT_METER_INVERSION
    meter = 1023 - meter;
#endif
    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 = 200;
    static int spear_state = 0;
    static int spear_timeout_count = 0;
    signed int duty = 0;
    int sensor = spear_sensor;  // リミット時に1
    
    switch(spear_state){
        case 0 : // 待ち
            if(spear){
                spear_state = 1;
            }
            else{
                if(sensor == 0){
                    duty = spearDuty * -1;
                }
            }
            break;
        case 1 : // 初期位置から抜け出す
            duty = spearDuty;
            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;
        case 3 : // 到達位置から抜け出す
            duty = spearDuty * -1;
            if(sensor == 0){
                spear_state = 4;
            }
            break;
        case 4 : // 初期位置まで戻る
            if(sensor == 0){
                duty = spearDuty * -1;
            }
            else{
                spear_state = 0;
            }
            break;
        default :
            break;
    }
    drive_motor(spearAddr, duty);
} 


void control(){
    
    drive_motor(EMO_Addr, 1);
    
    omuni.set_speed(speed_x, speed_y, omega, f);
    omuni.drive();
    
    arm_control();
    lift_control();
    spear_control();
    
}

int main()
{
    bool ex_comTimeout_state = true;
    char currentState;
    
    pc.baud(115200);
    comMain.baud(19200);
    comSub.baud(115200);
    pc.printf("Hello!\n");
    comMain.attach(comMain_rx, Serial::RxIrq);
    comSub.attach(comSub_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(1, 3.0f, 0.07f, 0.05f);
    omuni.set_pid(2, 3.0f, 0.07f, 0.05f);
    
    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);
    
    comTimeout.attach_us(comTimeout_intr, 1000);
    
    led = 0;
    
    //esc_sound(0);
    /*
    beep_note(96, 150);
    beep_note(98, 150);
    beep_note(100, 220);
    */
    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();
        }
        else{
            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();
        }
    }
}

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(liftAddr, 0);
    drive_motor(spearAddr, 0);
    
    drive_motor(EMO_Addr, 0);
    
    beep_note(100, 100);
    beep_note(96, 100);
    
    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(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));
}


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