Kiko Ishimoto / Mbed 2 deprecated robocon2017mbed_nedoControl_R

nedo用　未検証

Dependencies:   mbed

main.cpp@0:306e2b77804f, 2017-11-10 (annotated)

Committer:
kikoaac
Date:
Fri Nov 10 14:34:26 2017 +0000
Revision:
0:306e2b77804f
nedo?

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kikoaac 0:306e2b77804f 1 #include "mbed.h"
kikoaac 0:306e2b77804f 2 #define R 0
kikoaac 0:306e2b77804f 3 #define L 1
kikoaac 0:306e2b77804f 4 #define SHOULDER 0
kikoaac 0:306e2b77804f 5 #define ELBOW 1
kikoaac 0:306e2b77804f 6 #define WRIST 2
kikoaac 0:306e2b77804f 7 #define ARMPIT 3
kikoaac 0:306e2b77804f 8 DigitalOut nReset(dp26);
kikoaac 0:306e2b77804f 9 #if 0
kikoaac 0:306e2b77804f 10 DigitalOut debugLED1(dp24);
kikoaac 0:306e2b77804f 11 DigitalOut debugLED2(dp18);
kikoaac 0:306e2b77804f 12 #else
kikoaac 0:306e2b77804f 13 DigitalOut debugLED1(dp1);
kikoaac 0:306e2b77804f 14 DigitalOut debugLED2(dp2);
kikoaac 0:306e2b77804f 15 #endif
kikoaac 0:306e2b77804f 16 Timer timer;
kikoaac 0:306e2b77804f 17 class AnalogInLPF;
kikoaac 0:306e2b77804f 18 class AnalogInLPF : public AnalogIn
kikoaac 0:306e2b77804f 19 {
kikoaac 0:306e2b77804f 20 private:
kikoaac 0:306e2b77804f 21 float alpha;
kikoaac 0:306e2b77804f 22 float prevAnalog;
kikoaac 0:306e2b77804f 23 float nowAnalog;
kikoaac 0:306e2b77804f 24 public : AnalogInLPF(PinName pin,float alpha_ = 0.1) : AnalogIn(pin)
kikoaac 0:306e2b77804f 25 {
kikoaac 0:306e2b77804f 26 alpha = alpha_;
kikoaac 0:306e2b77804f 27 prevAnalog = 0.0;
kikoaac 0:306e2b77804f 28 }
kikoaac 0:306e2b77804f 29 float read(){
kikoaac 0:306e2b77804f 30 nowAnalog = AnalogIn::read();
kikoaac 0:306e2b77804f 31 nowAnalog = nowAnalog*alpha + (1-alpha)*prevAnalog;
kikoaac 0:306e2b77804f 32 prevAnalog = nowAnalog;
kikoaac 0:306e2b77804f 33 return nowAnalog;
kikoaac 0:306e2b77804f 34 }
kikoaac 0:306e2b77804f 35 short read_u16(){
kikoaac 0:306e2b77804f 36 nowAnalog = AnalogIn::read();
kikoaac 0:306e2b77804f 37 nowAnalog = nowAnalog*alpha + (1-alpha)*prevAnalog;
kikoaac 0:306e2b77804f 38 prevAnalog = nowAnalog;
kikoaac 0:306e2b77804f 39 return short(nowAnalog*0xFFFF);
kikoaac 0:306e2b77804f 40 }
kikoaac 0:306e2b77804f 41 };
kikoaac 0:306e2b77804f 42 uint16_t map(uint16_t in, uint16_t inMin, uint16_t inMax, uint16_t outMin, uint16_t outMax);
kikoaac 0:306e2b77804f 43 AnalogIn ArmSense[4] = {AnalogIn(dp10),AnalogIn(dp9),AnalogIn(dp11),AnalogIn(dp13)};
kikoaac 0:306e2b77804f 44 void waitTime(float ti){
kikoaac 0:306e2b77804f 45 Timer t;
kikoaac 0:306e2b77804f 46 t.start();
kikoaac 0:306e2b77804f 47 while(ti > t.read());
kikoaac 0:306e2b77804f 48 t.stop();
kikoaac 0:306e2b77804f 49 return;
kikoaac 0:306e2b77804f 50 }
kikoaac 0:306e2b77804f 51 //AnalogInLPF ArmSense[4] = {AnalogInLPF(dp10),AnalogInLPF(dp9),AnalogInLPF(dp11),AnalogInLPF(dp13)};
kikoaac 0:306e2b77804f 52 Serial dev(dp16,dp15);
kikoaac 0:306e2b77804f 53 #define dataNum 12
kikoaac 0:306e2b77804f 54 union floatInByte
kikoaac 0:306e2b77804f 55 {
kikoaac 0:306e2b77804f 56 uint16_t si;
kikoaac 0:306e2b77804f 57 unsigned char c[2];
kikoaac 0:306e2b77804f 58 };
kikoaac 0:306e2b77804f 59 char *tmp;
kikoaac 0:306e2b77804f 60 void RX(){
kikoaac 0:306e2b77804f 61 if(dev.getc() == 'L'){
kikoaac 0:306e2b77804f 62 timer.reset();
kikoaac 0:306e2b77804f 63 }
kikoaac 0:306e2b77804f 64 }
kikoaac 0:306e2b77804f 65 void reset(){
kikoaac 0:306e2b77804f 66 nReset = 0;
kikoaac 0:306e2b77804f 67 waitTime(0.001);
kikoaac 0:306e2b77804f 68 nReset = 1;
kikoaac 0:306e2b77804f 69 }
kikoaac 0:306e2b77804f 70 int main() {
kikoaac 0:306e2b77804f 71 nReset = true;
kikoaac 0:306e2b77804f 72 tmp = new char[dataNum];
kikoaac 0:306e2b77804f 73 debugLED1 = 1;
kikoaac 0:306e2b77804f 74 debugLED2 = 0;
kikoaac 0:306e2b77804f 75 for(int i = 0 ; i < 50; i++){
kikoaac 0:306e2b77804f 76 debugLED2 = !debugLED2;
kikoaac 0:306e2b77804f 77 waitTime(0.1);
kikoaac 0:306e2b77804f 78 }
kikoaac 0:306e2b77804f 79 dev.baud(115200);
kikoaac 0:306e2b77804f 80 dev.attach(RX,Serial::RxIrq);
kikoaac 0:306e2b77804f 81 timer.start();
kikoaac 0:306e2b77804f 82 while(1) {
kikoaac 0:306e2b77804f 83 //送信データ格納
kikoaac 0:306e2b77804f 84 debugLED1 = 1;
kikoaac 0:306e2b77804f 85 tmp[0] = '0';
kikoaac 0:306e2b77804f 86 tmp[1] = 0;
kikoaac 0:306e2b77804f 87 tmp[2] = 0;
kikoaac 0:306e2b77804f 88 tmp[3] = 0;
kikoaac 0:306e2b77804f 89 for(int i = 0 ;i < 4 ; i++){
kikoaac 0:306e2b77804f 90 uint16_t in = ArmSense[i].read_u16();
kikoaac 0:306e2b77804f 91 floatInByte temp;
kikoaac 0:306e2b77804f 92 temp.si = in;
kikoaac 0:306e2b77804f 93 tmp[4 + i*2] = temp.c[0];//マスター片腕
kikoaac 0:306e2b77804f 94 tmp[5 + i*2] = temp.c[1]; //マスター片腕
kikoaac 0:306e2b77804f 95 }
kikoaac 0:306e2b77804f 96 //送信データを送る
kikoaac 0:306e2b77804f 97 char *SerialData = tmp;
kikoaac 0:306e2b77804f 98 for(int i = 0 ; i < dataNum ; i++){
kikoaac 0:306e2b77804f 99 dev.putc(SerialData[i]);
kikoaac 0:306e2b77804f 100 }
kikoaac 0:306e2b77804f 101 while(timer.read_ms() > 1000){
kikoaac 0:306e2b77804f 102 if(timer.read_ms() > 3000){
kikoaac 0:306e2b77804f 103 reset();
kikoaac 0:306e2b77804f 104 break;
kikoaac 0:306e2b77804f 105 }
kikoaac 0:306e2b77804f 106 }
kikoaac 0:306e2b77804f 107 debugLED1 = 0;
kikoaac 0:306e2b77804f 108 wait_ms(15);
kikoaac 0:306e2b77804f 109 //delete SerialData;
kikoaac 0:306e2b77804f 110 }
kikoaac 0:306e2b77804f 111
kikoaac 0:306e2b77804f 112 delete tmp;
kikoaac 0:306e2b77804f 113 }
kikoaac 0:306e2b77804f 114 uint16_t map(uint16_t in, uint16_t inMin, uint16_t inMax, uint16_t outMin, uint16_t outMax) {
kikoaac 0:306e2b77804f 115 // check it's within the range
kikoaac 0:306e2b77804f 116 if (inMin<inMax) {
kikoaac 0:306e2b77804f 117 if (in <= inMin)
kikoaac 0:306e2b77804f 118 return outMin;
kikoaac 0:306e2b77804f 119 if (in >= inMax)
kikoaac 0:306e2b77804f 120 return outMax;
kikoaac 0:306e2b77804f 121 } else { // cope with input range being backwards.
kikoaac 0:306e2b77804f 122 if (in >= inMin)
kikoaac 0:306e2b77804f 123 return outMin;
kikoaac 0:306e2b77804f 124 if (in <= inMax)
kikoaac 0:306e2b77804f 125 return outMax;
kikoaac 0:306e2b77804f 126 }
kikoaac 0:306e2b77804f 127 // calculate how far into the range we are
kikoaac 0:306e2b77804f 128 float scale = float(in-inMin)/float(inMax-inMin);
kikoaac 0:306e2b77804f 129 // calculate the output.
kikoaac 0:306e2b77804f 130 return uint16_t(outMin + scale*(outMax-outMin));
kikoaac 0:306e2b77804f 131 }