File content as of revision 1:b178e785986d:

#include "mbed.h"
#include <math.h>

#include "BNO055.h"

#define DIGITS 8
//円周率
#define PI 3.14159265358979
//モータードライバ
#define HF_MOTRE 0x01
#define HB_MOTRE 0x02
#define MF_MOTRE 0x03
#define MB_MOTRE 0x04
#define ARM_UD 0x05
#define ARM_OC 0x06
#define RERAY 0x07
#define R1          0x0A
//モータードライバへの命令
#define STOP 0
#define CW 1
#define CCW -1
#define bSTOP 10
#define bCW 11
#define bCCW -11

#define sCW 21
#define sCCW -21
#define shCW 31
#define shCCW -31

DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
//DigitalOut ledx(LED4);
Serial serial(P0_15, P0_1);
Serial s485(P0_26, P0_16);
DigitalOut DE(P0_27);

AnalogIn test(P0_4);

//Serial serial2(p28, p27);
I2C md_i2c_tx(P0_11, P0_10);
Ticker timer;
Serial pc(USBTX, USBRX);

BNO055 imu(dp15, dp25);
double yaw, roll, pitch;


int cnt_now = -1;
volatile unsigned char dualshock_cmd_getflag = 0;
volatile unsigned char dualshock_data[9] = {0x00, 0x00, 0x00, 0x80, 0x80, 0x80, 0x80};
volatile unsigned char dualshock_data_number = 0;

uint16_t cnt1 = 0;
volatile unsigned char dualshock_cmd_getflag1 = 0;
volatile unsigned char dualshock_data1[9] = {0x00, 0x00, 0x00, 0x80, 0x80, 0x80, 0x80};
volatile unsigned char dualshock_data_number1 = 0;

uint16_t cnt2 = 0;
volatile unsigned char dualshock_cmd_getflag2 = 0;
volatile unsigned char dualshock_data2[9] = {0x00, 0x00, 0x00, 0x80, 0x80, 0x80, 0x80};
volatile unsigned char dualshock_data_number2 = 0;
char str[20];
char usart_rx = 0;
double lx, ly, lraw_rad, lclearance, ldistance, langle;
double rx, ry, rraw_rad, rclearance, rdistance, rangle;

void cv2bin( unsigned char n ) {
    int i;

    for( i = DIGITS-1; i >= 0; i-- ) {
        printf( "%d", ( n >> i ) & 1 );
    }
    printf( "\n" );
}

void joy_cal() {
    //以下ジョイスティック計算
    rx = -(128-dualshock_data[3])*2;
    ry = (128-dualshock_data[4])*2;
    if (rx >= 255) rx = 255;
    if (rx <= -255) rx = -255;
    if (ry >= 255) ry = 255;
    if (ry <= -255) ry = -255;
    rraw_rad = atan2(ry, rx)/PI;
    rdistance = hypot(rx, ry) - rclearance;
    rdistance *= (255 / (255 - rclearance));
    if (rdistance < 0) rdistance = 0;
    if (rdistance >= 255) rdistance = 255;
    rangle = (rraw_rad < 0 ? 2 + rraw_rad : rraw_rad) * 180;
    rangle = rdistance == 0 ? 0 : rangle;

    if (rx > 0) {
        rx -= rclearance;
        if (rx < 0) rx = 0;
    } else if (rx < 0) {
        rx += rclearance;
        if (rx > 0) rx = 0;
    }
    rx *= (255 / (255 - rclearance));
    
    lx = -(128-dualshock_data[5])*2;
    ly = (128-dualshock_data[6])*2;
    if (lx >= 255) lx = 255;
    if (lx <= -255) lx = -255;
    if (ly >= 255) ly = 255;
    if (ly <= -255) ly = -255;
    lraw_rad = atan2(ly, lx)/PI;
    ldistance = hypot(lx, ly) - lclearance;
    ldistance *= (255 / (255 - lclearance));
    if (ldistance < 0) ldistance = 0;
    if (ldistance >= 255) ldistance = 255;
    langle = (lraw_rad < 0 ? 2 + lraw_rad : lraw_rad) * 180;
    langle = ldistance == 0 ? 0 : langle;
}

//タイマ割り込み
void time() {
    
    if (dualshock_cmd_getflag & 0x02) {
        dualshock_cmd_getflag = 0;
        dualshock_data[1] = 0x00;
        dualshock_data[2] = 0x00;
        dualshock_data[3] = 0x80;
        dualshock_data[4] = 0x80;
        dualshock_data[5] = 0x80;
        dualshock_data[6] = 0x80;
        led2 = 0;
    }
    else
        dualshock_cmd_getflag |= 0x02;
}


void usart(){
    unsigned char rxdata;

    rxdata = serial.getc();
    serial.putc(dualshock_data_number);
    if (dualshock_data_number == 0 && rxdata == 'S')
        dualshock_data_number++;
    else if(dualshock_data_number >= 1 && dualshock_data_number <= 6) {
        if (dualshock_data_number <= 2)
            dualshock_data[dualshock_data_number] = ~rxdata;
        else
            dualshock_data[dualshock_data_number] = rxdata;
        dualshock_data_number++;
    }
    else if (dualshock_data_number == 7 && rxdata == 'E') {
        dualshock_cmd_getflag = 0x01;
        dualshock_data_number = 0;
        led2 = 1;
        joy_cal();
    }
    //ledx = !ledx;
}

void ics() {
    wait_ms(1);
    led3 = 0;
    //EN_IN = 0;
}


//i2c通信
void md_setoutput(unsigned char address, int rotate, int duty) {
    char val;
    char data[2];

        /* H30新型：処理最適化 */
        switch (rotate) {
            case CW:    data[0] = 'F'; break;// 電圧比例制御モード（PWMがLの区間はフリー）
            case CCW:   data[0] = 'R'; break;
            case STOP:  data[0] = 'S'; break;

            case bCW:   data[0] = 'f'; break; // 電圧比例制御モード（PWMがLの区間はブレーキ）
            case bCCW:  data[0] = 'r'; break;
            case bSTOP: data[0] = 's'; break;
            
            case sCW:   data[0] = 'A'; break; // 速度比例制御モード
            case sCCW:  data[0] = 'B'; break;
            
            case shCW:   data[0] = 'a'; break; // 速度比例制御モード（命令パケット最上位ビットに指令値9ビット目を入れることで分解能2倍）
            case shCCW:  data[0] = 'b'; break;
        }
            
    data[1] = duty & 0xFF;
    
    val = md_i2c_tx.write(address << 1, data, 2); //duty送信
}

void set_pos(int id, float angle) {
    float pos = 0;
    int data = 0; //3500～11500
    angle += 135;
    pos = (8000 / 270) * angle;
    data = (int)(pos > 0.5 ? pos + 0.5 : pos - 0.5);
    data += 3500;
    
    //int CMD = 0b10000000 + id;
    //char pos_H = (uint16_t)((data & 0b11111110000000) >> 7);
    //char pos_L = (uint16_t)(data & 0b00000001111111);
    

    led3 = 1;
    //EN_IN = 1;
    //servo.putc(CMD);
    //servo.putc(pos_H);
    //servo.putc(pos_L);
}

int main() {
    wait_ms(10);
    unsigned char stick_gain = 4;
    //led1 = 1;
    led2 = 0;
    led3 = 0;
    //ledx = 1;
    serial.baud(19200);//シリアル通信
    s485.baud(19200);


    //serial2.baud(19200);//シリアル通信
    pc.baud(230400);

    md_i2c_tx.frequency(20000000UL/(16 + 2 * 2 * 16)/2);//I2C通信
    timer.attach_us(time, 100000); //タイマ割り込み
    serial.attach(&usart, Serial::RxIrq); //受信割り込み
    //serial2.attach(&usart2, Serial::RxIrq); //受信割り込み

    //EN_IN = 0;
    //servo.baud(115200);
    //servo.format(8, Serial::Even, 1);
    //servo.attach(&ics, Serial::TxIrq);

    
    imu.reset();
    imu.check();


    // テスト用回転モード切り替え記録
    int Mcw = CW;
    int Mccw = CCW;
    int Mstop = STOP;
    
    lclearance = 60; //平行移動
    rclearance = 90; //旋回
    float power_up = 0;
    while (1) {
        DE = 1;
        s485.printf("S");
        //DE = 0;
        //led1 = EMS_STATE;
        //set_pos(0, -65);
        //CS = 0;
        //isoSPI.write(165);
        //CS = 1;
        /********************以下速度アップ処理********************/
        if (dualshock_data[2] & 0x04) power_up = 1;
        else power_up = 0.8;
        /********************以下メカナムメイン処理********************/
        if (rx > 0) {
            ////////////////////頭を右方向に旋回
            md_setoutput(HF_MOTRE,Mcw,rx*power_up);
            md_setoutput(HB_MOTRE,Mcw,rx*power_up);
            md_setoutput(MF_MOTRE,Mcw,rx*power_up);
            md_setoutput(MB_MOTRE,Mcw,rx*power_up);
        } else if (rx < 0) {
            ////////////////////頭を左方向に旋回
            md_setoutput(HF_MOTRE,Mccw,-rx*power_up);
            md_setoutput(HB_MOTRE,Mccw,-rx*power_up);
            md_setoutput(MF_MOTRE,Mccw,-rx*power_up);
            md_setoutput(MB_MOTRE,Mccw,-rx*power_up);
        } else {
            ////////////////////旋回をしない場合平行移動を行う
            //
            if ((ldistance != 0) && ((langle < 45) || (langle >= 315))){ //右
                md_setoutput(HF_MOTRE,Mcw,ldistance*power_up);
                md_setoutput(HB_MOTRE,Mcw,ldistance*power_up);
                md_setoutput(MF_MOTRE,Mccw,ldistance*power_up);
                md_setoutput(MB_MOTRE,Mccw,ldistance*power_up);
            } else if ((ldistance != 0) && (langle < 135)) { //前
                md_setoutput(HF_MOTRE,Mccw,ldistance*power_up);
                md_setoutput(HB_MOTRE,Mcw,ldistance*power_up);
                md_setoutput(MF_MOTRE,Mccw,ldistance*power_up);
                md_setoutput(MB_MOTRE,Mcw,ldistance*power_up);
            } else if ((ldistance != 0) && (langle < 225)) { //左
                md_setoutput(HF_MOTRE,Mccw,ldistance*power_up);
                md_setoutput(HB_MOTRE,Mccw,ldistance*power_up);
                md_setoutput(MF_MOTRE,Mcw,ldistance*power_up);
                md_setoutput(MB_MOTRE,Mcw,ldistance*power_up);
            } else if ((ldistance != 0) && (langle < 315)) { //後
                md_setoutput(HF_MOTRE,Mcw,ldistance*power_up);
                md_setoutput(HB_MOTRE,Mccw,ldistance*power_up);
                md_setoutput(MF_MOTRE,Mcw,ldistance*power_up);
                md_setoutput(MB_MOTRE,Mccw,ldistance*power_up);
            } else { //停止
                md_setoutput(HF_MOTRE,Mstop,0);
                md_setoutput(HB_MOTRE,Mstop,0);
                md_setoutput(MF_MOTRE,Mstop,0);
                md_setoutput(MB_MOTRE,Mstop,0);
            }
        }

        /* 回転モード設定（テスト用） */
        if (dualshock_data[2] & 0x40) { //通常フリー
            Mcw = CW;
            Mccw = CCW;
            Mstop = STOP;
            led1 = 0;
        } else if (dualshock_data[2] & 0x20) { //通常ブレーキ
            Mcw = bCW;
            Mccw = bCCW;
            Mstop = bSTOP;
            led1 = 1;
        } else if (dualshock_data[2] & 0x10) { //速調
            Mcw = sCW;
            Mccw = sCCW;
            Mstop = bSTOP;
            led1 = 1;
        }
        {
            imu.setmode(OPERATION_MODE_IMUPLUS);
            imu.get_calib();
            imu.get_angles();
            imu.get_quat();
        
            double q1, q2, q3, q4;
            q1 = imu.quat.w;
            q2 = imu.quat.x;
            q3 = imu.quat.y;
            q4 = imu.quat.z;

            double q3q3 = q3 * q3;
  
            double m1 = +2.0 * (q1 * q2 + q3 * q4);
            double m2 = +1.0 - 2.0 * (q2 * q2 + q3q3);
            //roll = atan2(m1, m2) * 57.2957795131;

            m1 = +2.0 * (q1 * q3 - q4 * q2);
            m1 = (m1 > 1.0)? 1.0 : m1;
            m1 = (m1 < -1.0)? -1.0 : m1;
            //pitch = asin(m1) * 57.2957795131;
  
            m1 = +2.0 * (q1 * q4 + q2 * q3);
            m2 = +1.0 - 2.0 * (q3q3 + q4 * q4);
            //yaw = atan2(m1, m2) * 57.2957795131;
        
            //pc.printf("yaw\t = %05d, roll\t = %05d, pitch\t = %05d\r\n", (int)yaw, (int)roll, (int)pitch);
            pc.printf("%05d\r\n", test.read_u16());
        }
        
        wait_ms(10);
    }
}