2018年度用翼端mbedプログラム
Dependencies: Control_Yokutan_CANver1 XBusServo mbed mbed-rtos
Fork of ControlYokutan2017_2 by
Diff: main.cpp
- Revision:
- 0:e052602db102
- Child:
- 1:9cc932a16d17
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Jan 29 13:38:29 2016 +0000 @@ -0,0 +1,122 @@ +//翼端can program +#include "mbed.h" +#include "ADXL345_I2C.h" +#include "INA226.hpp" +#define TO_SEND_DATAS_NUM 5 +#define INIT_SERVO_PERIOD_MS 20000 +#define WAIT_LOOP_TIME_MS 10 +#define CONTROL_VALUES_NUM 2 +#define TO_SEND_CAN_ID 100 +CAN can(p30,p29); +CANMessage recmsg; +Serial pc(USBTX,USBRX); +ADXL345_I2C accelerometer(p9, p10); +I2C ina226_i2c(p28,p27); +INA226 VCmonitor(ina226_i2c); +PwmOut servo1(p21); +PwmOut servo2(p22); + +char toSendDatas[TO_SEND_DATAS_NUM]; +char controlValues[CONTROL_VALUES_NUM]; + +int counter = 0; +int eruron_deg = 0; +int drug_deg = 0; +unsigned short ina_val; +double V,C; +bool SERVO_FLAG; +bool ADXL_FLAG; +bool INA_FLAG; + +int acc[3] = {0,0,0}; + +bool servoInit(){ + servo1.period_ms(INIT_SERVO_PERIOD_MS); + servo2.period_ms(INIT_SERVO_PERIOD_MS); + return true; +} + +bool adxlInit(){ + accelerometer.setPowerControl(0x00); + accelerometer.setDataFormatControl(0x0B); + accelerometer.setDataRate(ADXL345_3200HZ); + accelerometer.setPowerControl(0x08); + return true; +} + +bool inaInit(){ + if(!VCmonitor.isExist()){ + pc.printf("VCmonitor NOT FOUND\n"); + return false; + } + ina_val = 0; + if(VCmonitor.rawRead(0x00,&ina_val) != 0){ + pc.printf("VCmonitor READ ERROR\n"); + return false; + } + VCmonitor.setCurrentCalibration(); + return true; +} + +void init(){ + pc.printf("Receiver\n\r"); + SERVO_FLAG = servoInit(); + ADXL_FLAG = adxlInit(); + INA_FLAG = inaInit(); +} + +void updateDatas(){ + 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]; + } + toSendDatas[3] = (char)(V/100); + toSendDatas[4] = (char)(C/0100); +} + +void receiveDatas(){ + if(can.read(recmsg)){ + for(int i = 0; i < recmsg.len; i++){ + controlValues[i] = recmsg.data[i]; + } + } +} + +void toString(){ + //for(int i = 0; i < TO_SEND_DATAS_NUM; i++){ + pc.printf("%i:",toSendDatas[3]); + pc.printf("%i:",toSendDatas[4]); + pc.printf("%f:",V); + pc.printf("%f:",C); + //} + //for(int i = 0; i <CONTROL_VALUES_NUM; i++){ +// pc.printf("%i:",controlValues[i]); +// } +// pc.printf("%d",INA_FLAG); + pc.printf("\n\r"); +} + +void sendDatas(){ + if(can.write(CANMessage(TO_SEND_CAN_ID, toSendDatas, TO_SEND_DATAS_NUM))) + pc.printf("resend suc\n\r"); +} + +void + +int main(){ + init(); + while(1){ + receiveDatas(); + updateDatas(); + //WriteServo(); + toString(); + sendDatas(); + wait_ms(WAIT_LOOP_TIME_MS); + } +} \ No newline at end of file