File content as of revision 1:baab5b88a142:

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

const float omega_max = 1.0 * 2 * 3.14159265f;
const float wrap_radius = 1.5f;
const float wrap_speed = 2.0f;
const int8_t armDuty[] = {-50, -50};
const signed int spearDuty = 127;
const int32_t lift_preset[4] = {620, 990, 1360, 1730}; // 12bit 0 ~ 4095
const int32_t lift_max = 1830;
const int32_t lift_min = 500;

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

int drive_motor(int address,signed int duty);
void emergencyStop();

bool ems = false;

bool arm = false;
bool spear = false;
int lift_targ = lift_preset[0];
bool first_receive = true;
int past_lift_gray = 0b00;
int lift_inc = 0;

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

Serial p(USBTX, USBRX);
Serial pc(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;

float speed_x, speed_y, omega;
bool f;

typedef union{
    struct{
        unsigned angle   :4;
        unsigned speed   :2;
        unsigned byteNum0:2;
    };
    struct{
        unsigned wrap_R   :1;
        unsigned wrap_L   :1;
        unsigned coorSys  :1;
        unsigned omega    :2;
        unsigned dir      :1;
        unsigned byteNum1 :2;
    };
    struct{
        unsigned lift_down  :1;
        unsigned lift_up    :1;
        unsigned lift_gray  :2;
        unsigned spear      :1;
        unsigned arm        :1;
        unsigned byteNum2   :2;
    };
    struct{
        unsigned reserved2 :1;
        unsigned emo       :1;
        unsigned reserved3 :4;
        unsigned byteNum3  :2;
    };
    struct{
        unsigned control :6;
        unsigned byteNum :2;
    };
    struct{
        unsigned bit0:1;
        unsigned bit1:1;
        unsigned bit2:1;
        unsigned bit3:1;
        unsigned bit4:1;
        unsigned bit5:1;
        unsigned bit6:1;
        unsigned bit7:1;
    };
    int8_t data;
}comData_t;


comData_t RCData[4] = {0};

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


void pc_rx()
{
    char temp = pc.getc();
    comData_t rcTemp;
    rcTemp.data = temp;
    float rad, speed;
    int lift_nearest_num = 0;
    
    
    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");
    */
    //p.printf("%d\n", rcTemp.byteNum);
    //p.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);
            break;
        case 1:
            //p.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);
            break;
        case 3:
            
            break;
    }
    
    if(rcTemp.byteNum == 3){ // データは揃った
        
        led = !led;
        
        ems = RCData[3].emo != 0;
        
        rad = RCData[0].angle * 3.141592 / 8.0;
        speed = 0.8 * RCData[0].speed;
        speed_x = speed * cos(rad) * -1;
        speed_y = speed * sin(rad) * -1;
        
        f = RCData[1].coorSys;
        omega = RCData[1].omega * omega_max / 3.0f;
        omega *= RCData[1].dir ? 1 : -1;
        
        
        if(RCData[1].wrap_R || RCData[1].wrap_L){
            speed_x = wrap_speed;
            speed_y = 0;
            f = 0;
            omega = wrap_speed / wrap_radius;
            speed_x *= RCData[1].wrap_L ? 1 : -1;
            omega *= RCData[1].wrap_L ? 1 : -1;
        }
        
        
        arm = RCData[2].arm != 0;
        
        spear = RCData[2].spear != 0;      
        
        if(first_receive){
            past_lift_gray = RCData[2].lift_gray;
            first_receive = false;
        }
        if(RCData[2].lift_gray != past_lift_gray){ // 段階移動
            int lift_diff = 0;
            int lift_nearest = 10000;
            lift_diff = grayDiffer2bit(RCData[2].lift_gray, past_lift_gray);
            //p.printf("%d --> %d\n", past_lift_gray, RCData[2].lift_gray);
            //p.printf("gray = %d\n", lift_diff);
            for(int num = 0; num < 4; num++){ // 最寄りの段階
                //p.printf("nearest = %d, num = %d\n", lift_nearest, lift_nearest_num);
                if(lift_nearest > abs(lift_preset[num] - lift_targ)){
                    lift_nearest_num = num;
                    lift_nearest = abs(lift_preset[num] - lift_targ);
                }
            }
            //p.printf("lift_targ = %d num = %d\n", lift_targ, lift_nearest_num);
            if(lift_diff == 1){
                if(lift_preset[lift_nearest_num] >= lift_targ){
                    if(lift_nearest_num < 3)lift_targ = lift_preset[lift_nearest_num + 1];
                }
                else{
                    lift_targ = lift_preset[lift_nearest_num];
                }
            }
            else if(lift_diff == -1){
                if(lift_preset[lift_nearest_num] <= lift_targ){
                    if(lift_nearest_num > 0)lift_targ = lift_preset[lift_nearest_num - 1];
                }
                else{
                    lift_targ = lift_preset[lift_nearest_num];
                }
            }
        }
        past_lift_gray = RCData[2].lift_gray;
        
        if(RCData[2].lift_down != 0)lift_inc = -5;
        else if(RCData[2].lift_up != 0)lift_inc = 5;
        else lift_inc = 0;
        
        //p.printf("inc = %d\n",lift_inc);
        //p.printf("lift_targ = %d\n", lift_targ);
        
    } // すっきりした // してない
    
}


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(){
    int32_t meter = 0;
    signed int duty = 0;
    
    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;
    }
    meter = lift_meter.read_u16() >> 4;
    duty = lift_pid.get_pid(meter, lift_targ);
    drive_motor(liftAddr, duty);
    
}

void spear_control(){
    static int spear_state = 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){
                spear_state = 2;
            }
            break;
        case 2 : // 到達位置まで動かす
            if(sensor == 0){
                duty = spearDuty;
            }
            else{
                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(){
    
    arm_control();
    lift_control();
    spear_control();
    
}

int main()
{
    int period_us = 0;
    int count = 0;
    
    p.baud(115200);
    pc.baud(9600);
    p.printf("Hello!\n");
    pc.attach(pc_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);
    
    lift_pid.param(0.3, 0.0, 0.0);
    lift_pid.period(0.001);
    lift_pid.output(-127, 127);
    
    //control_loop.attach_us(&control, 1000);
    
    led = 0;
    
    //mesPeriod.start();
    
    while(1)
    {
        wait(0.0001);
        omuni.set_speed(speed_x, speed_y, omega, f);
        omuni.drive();
        control();
        
        if(ems){
            emergencyStop();
        }
        /*
        if(count < 100){
            count += 1;
        }
        else{
            period_us = mesPeriod.read_us();
            mesPeriod.reset();
            
            p.printf("%dHz\n", 1000000 * 100 / period_us);
            
            count = 0;
        }
        */
    }
}

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);
    while(1);
}