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();