albatross / Mbed 2 deprecated XBusServoTest

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

Dependencies:   XBusServo mbed

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?

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 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 }