2018年度用翼端mbedプログラム
Dependencies: Control_Yokutan_CANver1 XBusServo mbed mbed-rtos
Fork of ControlYokutan2017_2 by
Diff: main.cpp
- Revision:
- 22:b38bc18ec3a1
- Parent:
- 21:8a621c81507d
- Child:
- 23:d551db88df65
- Child:
- 24:d416722b4aad
--- a/main.cpp Thu Oct 27 15:54:44 2016 +0000 +++ b/main.cpp Sat Jan 28 02:28:06 2017 +0000 @@ -1,9 +1,10 @@ //翼端can program #include "mbed.h" -#include "ADXL345_I2C.h" +//#include "ADXL345_I2C.h" +#include "MPU6050.h" #include "INA226.hpp" -#define TO_SEND_DATAS_NUM 4 +#define TO_SEND_DATAS_NUM 7 #define INIT_SERVO_PERIOD_MS 20 #define WAIT_LOOP_TIME 0.02 #define CONTROL_VALUES_NUM 2 @@ -25,7 +26,8 @@ CAN can(p30,p29); CANMessage recmsg; Serial pc(USBTX,USBRX); -ADXL345_I2C accelerometer(p9, p10); +//ADXL345_I2C accelerometer(p9, p10); +MPU6050 mpu(p9,p10); I2C ina226_i2c(p28,p27); INA226 VCmonitor(ina226_i2c); PwmOut drugServo(p22); @@ -41,7 +43,7 @@ DigitalOut led3(LED3); DigitalOut led4(LED4); Ticker sendDatasTicker; -Ticker toStringTicker; +//Ticker toStringTicker; Ticker receiveDatasTicker; char toSendDatas[TO_SEND_DATAS_NUM]; @@ -56,10 +58,12 @@ bool SERVO_FLAG; bool ADXL_FLAG; bool INA_FLAG; +bool MPU_FLAG; -int acc[3] = {0,0,0}; -char acc_mean[3][ADXL_MEAN_NUM]; -int adxl_mean_counter = 0; +//int16_t acc[3] = {0,0,0}; +char gyro_c[6] = {0,0,0,0,0,0}; +//char acc_mean[3][ADXL_MEAN_NUM]; +//int adxl_mean_counter = 0; void toString(); void receiveDatas(); @@ -71,13 +75,13 @@ return true; } -bool adxlInit(){ - accelerometer.setPowerControl(0x00); - accelerometer.setDataFormatControl(0x0B); - accelerometer.setDataRate(ADXL345_3200HZ); - accelerometer.setPowerControl(0x08); - return true; -} +//bool adxlInit(){ +// accelerometer.setPowerControl(0x00); +// accelerometer.setDataFormatControl(0x0B); +// accelerometer.setDataRate(ADXL345_3200HZ); +// accelerometer.setPowerControl(0x08); +// return true; +//} void sendDatas(){ if(can.write(CANMessage(TO_SEND_CAN_ID, toSendDatas, TO_SEND_DATAS_NUM))){ @@ -112,7 +116,8 @@ drugMoveDeg =DRUG_MOVE_DEG_INI_R; } SERVO_FLAG = servoInit(); - ADXL_FLAG = adxlInit(); +// ADXL_FLAG = adxlInit(); + MPU_FLAG = mpu.testConnection(); INA_FLAG = inaInit(); sendDatasTicker.attach(&sendDatas,SEND_DATAS_LOOP_TIME); // toStringTicker.attach(&toString,0.5); @@ -120,17 +125,22 @@ } void updateDatas(){ - if(ADXL_FLAG){ - accelerometer.getOutput(acc); - } +// if(ADXL_FLAG){ +//// accelerometer.getOutput(acc); +// } if(INA_FLAG){ int tmp = VCmonitor.getVoltage(&V); tmp = VCmonitor.getCurrent(&C); } - for(int i = 0; i < 3; i++){ - toSendDatas[i] = acc[i]; + if(MPU_FLAG){ + mpu.read(MPU6050_GYRO_XOUT_H_REG, gyro_c, 6); } - toSendDatas[3] = (char)(V/100); + for(int i = 0; i < TO_SEND_DATAS_NUM - 1; i++){ +// toSendDatas[i] = acc[i]; + toSendDatas[i] = gyro_c[i]; + } +// toSendDatas[TO_SEND_DATAS_NUM - 1] = (char)(V/100); + toSendDatas[TO_SEND_DATAS_NUM - 1] = (char)77; } void receiveDatas(){ @@ -220,8 +230,11 @@ debugLED = 0; //receiveDatas(); +// sendDatas(); WriteServo(); updateDatas(); +// pc.printf("%6d,%6d,%6d\n\r",gyro[0],gyro[1],gyro[2]); + led3 = !led3; wait(WAIT_LOOP_TIME); } } \ No newline at end of file