![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
test
Dependencies: mbed
Fork of BNO055_HelloWorld by
main.cpp
- Committer:
- SES01
- Date:
- 2018-05-22
- Revision:
- 1:b178e785986d
- Parent:
- 0:a8e6e26ebe0f
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); } }