ControlMainでの変更に対応して、新しくレポジトリを作りました
Dependencies: Control_Yokutan_CANver1 XBusServo mbed mbed-rtos
Fork of ControlYokutan2017 by
Diff: main.cpp
- Branch:
- XBus???
- Revision:
- 24:d416722b4aad
- Parent:
- 22:b38bc18ec3a1
- Child:
- 25:e8bfb629e1b1
--- a/main.cpp Sat Jan 28 02:28:06 2017 +0000 +++ b/main.cpp Wed Feb 15 13:27:52 2017 +0000 @@ -3,6 +3,7 @@ //#include "ADXL345_I2C.h" #include "MPU6050.h" #include "INA226.hpp" +#include "XBusServo.h" #define TO_SEND_DATAS_NUM 7 #define INIT_SERVO_PERIOD_MS 20 @@ -23,6 +24,13 @@ #define ERURON_TRIM_INI_L 113 //元々95 #define DRUG_TRIM_INI_L 110 +#define kXBusTx p9 +#define kMaxServoNum 1 // 1 - 50 +#define kMaxServoPause (sizeof(motionData) / sizeof(pauseRec)) +#define kMotionInterval 10 // flame / sec +#define kMotionMinMark 0x1249 +#define kMotionEndMark 0xED86 + CAN can(p30,p29); CANMessage recmsg; Serial pc(USBTX,USBRX); @@ -69,12 +77,30 @@ void receiveDatas(); void WriteServo(); +static const uint8_t servoChannel = 0x01; + +XBusServo gXBus(kXBusTx, NC, NC, kMaxServoNum); +Ticker gTimer; + bool servoInit(){ drugServo.period_ms(INIT_SERVO_PERIOD_MS); eruronServo.period_ms(INIT_SERVO_PERIOD_MS); return true; } +//============================================================= +// XbusIntervalHandler() +// play motion ! +//============================================================= +void XbusIntervalHandler() +{ + uint16_t value; + uint16_t diff = kMotionEndMark - kMotionMinMark; + value = (uint16_t)(diff * analog.read()) + kMotionMinMark; + gXBus.setServo(servoChannel, value); + gXBus.sendChannelDataPacket(); +} + //bool adxlInit(){ // accelerometer.setPowerControl(0x00); // accelerometer.setDataFormatControl(0x0B); @@ -102,7 +128,10 @@ return true; } -void init(){ +XBusError init(){ + + XBusError result; + if(!LRstatePin){ eruronTrim = ERURON_TRIM_INI_L; drugTrim = DRUG_TRIM_INI_L; @@ -122,6 +151,22 @@ sendDatasTicker.attach(&sendDatas,SEND_DATAS_LOOP_TIME); // toStringTicker.attach(&toString,0.5); receiveDatasTicker.attach(&receiveDatas,RECEIVE_DATAS_LOOP_TIME); + + //XBus + // initialize XBus + result = gXBus.start(); + if (result != kXBusError_NoError) { + gXBus.stop(); + return result; + } + // initialize XBus Servos + result = gXBus.addServo(servoChannel, kXbusServoNeutral); + if (result != kXBusError_NoError) { + gXBus.stop(); + return result; + } + + return kXBusError_NoError; } void updateDatas(){ @@ -216,6 +261,11 @@ int main(){ init(); + XBusError result; + + // start motion + gTimer.attach_us(&XbusIntervalHandler, 1000000 / kMotionInterval); + while(1){ while(setTrimPin){ setTrim();