albatross / Mbed 2 deprecated XBusServoTest

XBusServoの設定を行うプログラムです。

Dependencies:   XBusServo mbed

main.cpp@0:301100d3469a, 2017-01-04 (annotated)

Committer:
YusukeWakuta
Date:
Wed Jan 04 09:10:05 2017 +0000
Revision:
0:301100d3469a
Child:
1:fee82ed64991
Child:
2:32aba462e4df
????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
YusukeWakuta 0:301100d3469a 1 #include "mbed.h"
YusukeWakuta 0:301100d3469a 2 #include "XBusServo.h"
YusukeWakuta 0:301100d3469a 3 #include <string>
YusukeWakuta 0:301100d3469a 4
YusukeWakuta 0:301100d3469a 5 //#define DEBUGmain
YusukeWakuta 0:301100d3469a 6
YusukeWakuta 0:301100d3469a 7 #ifdef DEBUGmain
YusukeWakuta 0:301100d3469a 8 #define DBG(fmt) printf(fmt)
YusukeWakuta 0:301100d3469a 9 #define DBGF(fmt, ...) printf(fmt, __VA_ARGS__)
YusukeWakuta 0:301100d3469a 10 #else
YusukeWakuta 0:301100d3469a 11 #define DBG(...)
YusukeWakuta 0:301100d3469a 12 #define DBGF(...)
YusukeWakuta 0:301100d3469a 13 #endif
YusukeWakuta 0:301100d3469a 14
YusukeWakuta 0:301100d3469a 15 //
YusukeWakuta 0:301100d3469a 16 // defines
YusukeWakuta 0:301100d3469a 17 //
YusukeWakuta 0:301100d3469a 18 #define kXBusTx p9
YusukeWakuta 0:301100d3469a 19 #define kMaxServoNum 1 // 1 - 50
YusukeWakuta 0:301100d3469a 20
YusukeWakuta 0:301100d3469a 21 #define kMaxServoPause (sizeof(motionData) / sizeof(pauseRec))
YusukeWakuta 0:301100d3469a 22
YusukeWakuta 0:301100d3469a 23 #define kMotionInterval 30 // flame / sec
YusukeWakuta 0:301100d3469a 24 #define kMotionMinMark 0x1249
YusukeWakuta 0:301100d3469a 25 #define kMotionEndMark 0xED86
YusukeWakuta 0:301100d3469a 26
YusukeWakuta 0:301100d3469a 27 AnalogIn analog(p20);
YusukeWakuta 0:301100d3469a 28 Serial pc(USBTX,USBRX);
YusukeWakuta 0:301100d3469a 29
YusukeWakuta 0:301100d3469a 30 //
YusukeWakuta 0:301100d3469a 31 // typedefs
YusukeWakuta 0:301100d3469a 32 //
YusukeWakuta 0:301100d3469a 33 typedef enum poseDataType {
YusukeWakuta 0:301100d3469a 34 dataType_Pose,
YusukeWakuta 0:301100d3469a 35 numOfDataType
YusukeWakuta 0:301100d3469a 36 } poseDataType;
YusukeWakuta 0:301100d3469a 37
YusukeWakuta 0:301100d3469a 38
YusukeWakuta 0:301100d3469a 39 typedef struct poseRec {
YusukeWakuta 0:301100d3469a 40 poseDataType poseType;
YusukeWakuta 0:301100d3469a 41 uint16_t nextPoseIndex;
YusukeWakuta 0:301100d3469a 42 int32_t servoPosition[kMaxServoNum];
YusukeWakuta 0:301100d3469a 43 uint16_t frameFromBeforePause;
YusukeWakuta 0:301100d3469a 44 }
YusukeWakuta 0:301100d3469a 45 poseRec;
YusukeWakuta 0:301100d3469a 46
YusukeWakuta 0:301100d3469a 47 static const uint8_t servoChannel = 0x01;
YusukeWakuta 0:301100d3469a 48
YusukeWakuta 0:301100d3469a 49 //
YusukeWakuta 0:301100d3469a 50 // global vars
YusukeWakuta 0:301100d3469a 51 //
YusukeWakuta 0:301100d3469a 52 XBusServo gXBus(kXBusTx, NC, NC, kMaxServoNum);
YusukeWakuta 0:301100d3469a 53 Ticker gTimer;
YusukeWakuta 0:301100d3469a 54
YusukeWakuta 0:301100d3469a 55 string Deci2Hex(int val,bool lower = false){
YusukeWakuta 0:301100d3469a 56 string str = "0x";
YusukeWakuta 0:301100d3469a 57 char answer[10];
YusukeWakuta 0:301100d3469a 58 char henkan[17] = "0123456789ABCDEF";
YusukeWakuta 0:301100d3469a 59 int i = 0,j = 0;
YusukeWakuta 0:301100d3469a 60 while(val > 0){
YusukeWakuta 0:301100d3469a 61 j = val % 16;
YusukeWakuta 0:301100d3469a 62 val = val / 16;
YusukeWakuta 0:301100d3469a 63 answer[i] = henkan[j];
YusukeWakuta 0:301100d3469a 64 i++;
YusukeWakuta 0:301100d3469a 65 }
YusukeWakuta 0:301100d3469a 66 i--;
YusukeWakuta 0:301100d3469a 67 for(j = i;j >= 0;j--){
YusukeWakuta 0:301100d3469a 68 str += answer[j];
YusukeWakuta 0:301100d3469a 69 }
YusukeWakuta 0:301100d3469a 70 return str;
YusukeWakuta 0:301100d3469a 71 }
YusukeWakuta 0:301100d3469a 72
YusukeWakuta 0:301100d3469a 73 //=============================================================
YusukeWakuta 0:301100d3469a 74 // XbusIntervalHandler()
YusukeWakuta 0:301100d3469a 75 // play motion !
YusukeWakuta 0:301100d3469a 76 //=============================================================
YusukeWakuta 0:301100d3469a 77 void XbusIntervalHandler()
YusukeWakuta 0:301100d3469a 78 {
YusukeWakuta 0:301100d3469a 79 uint16_t value;
YusukeWakuta 0:301100d3469a 80 uint16_t diff = kMotionEndMark - kMotionMinMark;
YusukeWakuta 0:301100d3469a 81 // send current data
YusukeWakuta 0:301100d3469a 82 gXBus.sendChannelDataPacket();
YusukeWakuta 0:301100d3469a 83 value = (uint16_t)(diff * analog.read()) + kMotionMinMark;
YusukeWakuta 0:301100d3469a 84 gXBus.setServo(servoChannel, value);
YusukeWakuta 0:301100d3469a 85 }
YusukeWakuta 0:301100d3469a 86
YusukeWakuta 0:301100d3469a 87 //=============================================================
YusukeWakuta 0:301100d3469a 88 // init()
YusukeWakuta 0:301100d3469a 89 // initialize all setup
YusukeWakuta 0:301100d3469a 90 //=============================================================
YusukeWakuta 0:301100d3469a 91 XBusError init()
YusukeWakuta 0:301100d3469a 92 {
YusukeWakuta 0:301100d3469a 93 XBusError result;
YusukeWakuta 0:301100d3469a 94
YusukeWakuta 0:301100d3469a 95 // initialize XBus
YusukeWakuta 0:301100d3469a 96 result = gXBus.start();
YusukeWakuta 0:301100d3469a 97 if (result != kXBusError_NoError) {
YusukeWakuta 0:301100d3469a 98 gXBus.stop();
YusukeWakuta 0:301100d3469a 99 return result;
YusukeWakuta 0:301100d3469a 100 }
YusukeWakuta 0:301100d3469a 101
YusukeWakuta 0:301100d3469a 102 // initialize XBus Servos
YusukeWakuta 0:301100d3469a 103 result = gXBus.addServo(servoChannel, kXbusServoNeutral);
YusukeWakuta 0:301100d3469a 104 if (result != kXBusError_NoError) {
YusukeWakuta 0:301100d3469a 105 gXBus.stop();
YusukeWakuta 0:301100d3469a 106 return result;
YusukeWakuta 0:301100d3469a 107 }
YusukeWakuta 0:301100d3469a 108
YusukeWakuta 0:301100d3469a 109 return kXBusError_NoError;
YusukeWakuta 0:301100d3469a 110 }
YusukeWakuta 0:301100d3469a 111
YusukeWakuta 0:301100d3469a 112 //=============================================================
YusukeWakuta 0:301100d3469a 113 // main()
YusukeWakuta 0:301100d3469a 114 //
YusukeWakuta 0:301100d3469a 115 //=============================================================
YusukeWakuta 0:301100d3469a 116 int main()
YusukeWakuta 0:301100d3469a 117 {
YusukeWakuta 0:301100d3469a 118 if (init() != kXBusError_NoError)
YusukeWakuta 0:301100d3469a 119 return -1;
YusukeWakuta 0:301100d3469a 120
YusukeWakuta 0:301100d3469a 121 // start motion
YusukeWakuta 0:301100d3469a 122 gTimer.attach_us(&XbusIntervalHandler, 1000000 / kMotionInterval);
YusukeWakuta 0:301100d3469a 123
YusukeWakuta 0:301100d3469a 124 while(1) {
YusukeWakuta 0:301100d3469a 125 }
YusukeWakuta 0:301100d3469a 126 }