![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
XBusServoの設定を行うプログラムです。
main.cpp@1:fee82ed64991, 2017-01-07 (annotated)
- Committer:
- YusukeWakuta
- Date:
- Sat Jan 07 09:41:37 2017 +0000
- Revision:
- 1:fee82ed64991
- Parent:
- 0:301100d3469a
Arduino?????;
Who changed what in which revision?
User | Revision | Line number | New 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 | 1:fee82ed64991 | 29 | DigitalOut Led(LED1); |
YusukeWakuta | 0:301100d3469a | 30 | |
YusukeWakuta | 0:301100d3469a | 31 | // |
YusukeWakuta | 1:fee82ed64991 | 32 | // global vars |
YusukeWakuta | 0:301100d3469a | 33 | // |
YusukeWakuta | 1:fee82ed64991 | 34 | unsigned char gOK2SendPacket = true; |
YusukeWakuta | 1:fee82ed64991 | 35 | unsigned int gCurrentPos = kXbusServoNeutral; |
YusukeWakuta | 1:fee82ed64991 | 36 | long int gCurrentValue = 0; |
YusukeWakuta | 1:fee82ed64991 | 37 | unsigned char gCurrentID = 1; |
YusukeWakuta | 1:fee82ed64991 | 38 | unsigned char gCurrentSubID = 0; |
YusukeWakuta | 1:fee82ed64991 | 39 | uint8_t Angle = 0x1D; |
YusukeWakuta | 1:fee82ed64991 | 40 | int16_t AngleValue = 0x01; |
YusukeWakuta | 1:fee82ed64991 | 41 | static const uint8_t servoChannel = 0x01; |
YusukeWakuta | 0:301100d3469a | 42 | |
YusukeWakuta | 1:fee82ed64991 | 43 | uint16_t value; |
YusukeWakuta | 1:fee82ed64991 | 44 | uint16_t diff = kMotionEndMark - kMotionMinMark; |
YusukeWakuta | 0:301100d3469a | 45 | |
YusukeWakuta | 0:301100d3469a | 46 | // |
YusukeWakuta | 0:301100d3469a | 47 | // global vars |
YusukeWakuta | 0:301100d3469a | 48 | // |
YusukeWakuta | 0:301100d3469a | 49 | XBusServo gXBus(kXBusTx, NC, NC, kMaxServoNum); |
YusukeWakuta | 0:301100d3469a | 50 | Ticker gTimer; |
YusukeWakuta | 0:301100d3469a | 51 | |
YusukeWakuta | 0:301100d3469a | 52 | //============================================================= |
YusukeWakuta | 0:301100d3469a | 53 | // XbusIntervalHandler() |
YusukeWakuta | 0:301100d3469a | 54 | // play motion ! |
YusukeWakuta | 0:301100d3469a | 55 | //============================================================= |
YusukeWakuta | 0:301100d3469a | 56 | void XbusIntervalHandler() |
YusukeWakuta | 0:301100d3469a | 57 | { |
YusukeWakuta | 1:fee82ed64991 | 58 | if(gOK2SendPacket) { |
YusukeWakuta | 1:fee82ed64991 | 59 | value = (uint16_t)(diff * analog.read()) + kMotionMinMark; |
YusukeWakuta | 1:fee82ed64991 | 60 | gXBus.setServo(servoChannel, value); |
YusukeWakuta | 1:fee82ed64991 | 61 | |
YusukeWakuta | 1:fee82ed64991 | 62 | // send current data |
YusukeWakuta | 1:fee82ed64991 | 63 | gXBus.sendChannelDataPacket(); |
YusukeWakuta | 1:fee82ed64991 | 64 | } |
YusukeWakuta | 0:301100d3469a | 65 | } |
YusukeWakuta | 0:301100d3469a | 66 | |
YusukeWakuta | 0:301100d3469a | 67 | //============================================================= |
YusukeWakuta | 1:fee82ed64991 | 68 | // setCurrentValue() |
YusukeWakuta | 1:fee82ed64991 | 69 | // set current value to the servo for temp |
YusukeWakuta | 1:fee82ed64991 | 70 | //============================================================= |
YusukeWakuta | 1:fee82ed64991 | 71 | void setCurrentValue() |
YusukeWakuta | 1:fee82ed64991 | 72 | { |
YusukeWakuta | 1:fee82ed64991 | 73 | XBusError result; |
YusukeWakuta | 1:fee82ed64991 | 74 | for(;;) { |
YusukeWakuta | 1:fee82ed64991 | 75 | result = gXBus.setCommand(Angle, &AngleValue); |
YusukeWakuta | 1:fee82ed64991 | 76 | wait_ms(10); |
YusukeWakuta | 1:fee82ed64991 | 77 | if (result == kXBusError_NoError) |
YusukeWakuta | 1:fee82ed64991 | 78 | break; |
YusukeWakuta | 1:fee82ed64991 | 79 | } |
YusukeWakuta | 1:fee82ed64991 | 80 | } |
YusukeWakuta | 1:fee82ed64991 | 81 | |
YusukeWakuta | 1:fee82ed64991 | 82 | //============================================================= |
YusukeWakuta | 1:fee82ed64991 | 83 | // writeCurrentValue() |
YusukeWakuta | 1:fee82ed64991 | 84 | // write current value to the servo |
YusukeWakuta | 1:fee82ed64991 | 85 | //============================================================= |
YusukeWakuta | 1:fee82ed64991 | 86 | void writeCurrentValue() |
YusukeWakuta | 1:fee82ed64991 | 87 | { |
YusukeWakuta | 1:fee82ed64991 | 88 | XBusError result; |
YusukeWakuta | 1:fee82ed64991 | 89 | for (;;) { |
YusukeWakuta | 1:fee82ed64991 | 90 | result = gXBus.setCommand(Angle, &AngleValue); |
YusukeWakuta | 1:fee82ed64991 | 91 | wait_ms(10); |
YusukeWakuta | 1:fee82ed64991 | 92 | if (result == kXBusError_NoError) |
YusukeWakuta | 1:fee82ed64991 | 93 | break; |
YusukeWakuta | 1:fee82ed64991 | 94 | } |
YusukeWakuta | 1:fee82ed64991 | 95 | } |
YusukeWakuta | 1:fee82ed64991 | 96 | //============================================================= |
YusukeWakuta | 0:301100d3469a | 97 | // init() |
YusukeWakuta | 0:301100d3469a | 98 | // initialize all setup |
YusukeWakuta | 0:301100d3469a | 99 | //============================================================= |
YusukeWakuta | 0:301100d3469a | 100 | XBusError init() |
YusukeWakuta | 0:301100d3469a | 101 | { |
YusukeWakuta | 0:301100d3469a | 102 | XBusError result; |
YusukeWakuta | 0:301100d3469a | 103 | |
YusukeWakuta | 0:301100d3469a | 104 | // initialize XBus |
YusukeWakuta | 0:301100d3469a | 105 | result = gXBus.start(); |
YusukeWakuta | 0:301100d3469a | 106 | if (result != kXBusError_NoError) { |
YusukeWakuta | 0:301100d3469a | 107 | gXBus.stop(); |
YusukeWakuta | 0:301100d3469a | 108 | return result; |
YusukeWakuta | 0:301100d3469a | 109 | } |
YusukeWakuta | 0:301100d3469a | 110 | |
YusukeWakuta | 0:301100d3469a | 111 | // initialize XBus Servos |
YusukeWakuta | 0:301100d3469a | 112 | result = gXBus.addServo(servoChannel, kXbusServoNeutral); |
YusukeWakuta | 0:301100d3469a | 113 | if (result != kXBusError_NoError) { |
YusukeWakuta | 0:301100d3469a | 114 | gXBus.stop(); |
YusukeWakuta | 0:301100d3469a | 115 | return result; |
YusukeWakuta | 0:301100d3469a | 116 | } |
YusukeWakuta | 0:301100d3469a | 117 | return kXBusError_NoError; |
YusukeWakuta | 0:301100d3469a | 118 | } |
YusukeWakuta | 0:301100d3469a | 119 | |
YusukeWakuta | 0:301100d3469a | 120 | int main() |
YusukeWakuta | 0:301100d3469a | 121 | { |
YusukeWakuta | 0:301100d3469a | 122 | if (init() != kXBusError_NoError) |
YusukeWakuta | 0:301100d3469a | 123 | return -1; |
YusukeWakuta | 0:301100d3469a | 124 | |
YusukeWakuta | 0:301100d3469a | 125 | // start motion |
YusukeWakuta | 0:301100d3469a | 126 | gTimer.attach_us(&XbusIntervalHandler, 1000000 / kMotionInterval); |
YusukeWakuta | 0:301100d3469a | 127 | |
YusukeWakuta | 0:301100d3469a | 128 | while(1) { |
YusukeWakuta | 1:fee82ed64991 | 129 | gOK2SendPacket = false; |
YusukeWakuta | 1:fee82ed64991 | 130 | // setCurrentValue(); |
YusukeWakuta | 1:fee82ed64991 | 131 | // writeCurrentValue(); |
YusukeWakuta | 1:fee82ed64991 | 132 | |
YusukeWakuta | 1:fee82ed64991 | 133 | value = (uint16_t)(diff * analog.read()) + kMotionMinMark; |
YusukeWakuta | 1:fee82ed64991 | 134 | gXBus.setServo(servoChannel,value ); |
YusukeWakuta | 1:fee82ed64991 | 135 | gOK2SendPacket = true; |
YusukeWakuta | 0:301100d3469a | 136 | } |
YusukeWakuta | 0:301100d3469a | 137 | } |