Kiko Ishimoto
/
robocon2017mbed_nedoControl_R
nedo用 未検証
main.cpp
- Committer:
- kikoaac
- Date:
- 2017-11-10
- Revision:
- 0:306e2b77804f
File content as of revision 0:306e2b77804f:
#include "mbed.h" #define R 0 #define L 1 #define SHOULDER 0 #define ELBOW 1 #define WRIST 2 #define ARMPIT 3 DigitalOut nReset(dp26); #if 0 DigitalOut debugLED1(dp24); DigitalOut debugLED2(dp18); #else DigitalOut debugLED1(dp1); DigitalOut debugLED2(dp2); #endif Timer timer; class AnalogInLPF; class AnalogInLPF : public AnalogIn { private: float alpha; float prevAnalog; float nowAnalog; public : AnalogInLPF(PinName pin,float alpha_ = 0.1) : AnalogIn(pin) { alpha = alpha_; prevAnalog = 0.0; } float read(){ nowAnalog = AnalogIn::read(); nowAnalog = nowAnalog*alpha + (1-alpha)*prevAnalog; prevAnalog = nowAnalog; return nowAnalog; } short read_u16(){ nowAnalog = AnalogIn::read(); nowAnalog = nowAnalog*alpha + (1-alpha)*prevAnalog; prevAnalog = nowAnalog; return short(nowAnalog*0xFFFF); } }; uint16_t map(uint16_t in, uint16_t inMin, uint16_t inMax, uint16_t outMin, uint16_t outMax); AnalogIn ArmSense[4] = {AnalogIn(dp10),AnalogIn(dp9),AnalogIn(dp11),AnalogIn(dp13)}; void waitTime(float ti){ Timer t; t.start(); while(ti > t.read()); t.stop(); return; } //AnalogInLPF ArmSense[4] = {AnalogInLPF(dp10),AnalogInLPF(dp9),AnalogInLPF(dp11),AnalogInLPF(dp13)}; Serial dev(dp16,dp15); #define dataNum 12 union floatInByte { uint16_t si; unsigned char c[2]; }; char *tmp; void RX(){ if(dev.getc() == 'L'){ timer.reset(); } } void reset(){ nReset = 0; waitTime(0.001); nReset = 1; } int main() { nReset = true; tmp = new char[dataNum]; debugLED1 = 1; debugLED2 = 0; for(int i = 0 ; i < 50; i++){ debugLED2 = !debugLED2; waitTime(0.1); } dev.baud(115200); dev.attach(RX,Serial::RxIrq); timer.start(); while(1) { //送信データ格納 debugLED1 = 1; tmp[0] = '0'; tmp[1] = 0; tmp[2] = 0; tmp[3] = 0; for(int i = 0 ;i < 4 ; i++){ uint16_t in = ArmSense[i].read_u16(); floatInByte temp; temp.si = in; tmp[4 + i*2] = temp.c[0];//マスター片腕 tmp[5 + i*2] = temp.c[1]; //マスター片腕 } //送信データを送る char *SerialData = tmp; for(int i = 0 ; i < dataNum ; i++){ dev.putc(SerialData[i]); } while(timer.read_ms() > 1000){ if(timer.read_ms() > 3000){ reset(); break; } } debugLED1 = 0; wait_ms(15); //delete SerialData; } delete tmp; } uint16_t map(uint16_t in, uint16_t inMin, uint16_t inMax, uint16_t outMin, uint16_t outMax) { // check it's within the range if (inMin<inMax) { if (in <= inMin) return outMin; if (in >= inMax) return outMax; } else { // cope with input range being backwards. if (in >= inMin) return outMin; if (in <= inMax) return outMax; } // calculate how far into the range we are float scale = float(in-inMin)/float(inMax-inMin); // calculate the output. return uint16_t(outMin + scale*(outMax-outMin)); }