main.cpp

00001 #include "mbed.h"
00002 #define R 0
00003 #define L 1
00004 #define SHOULDER 0
00005 #define ELBOW 1
00006 #define WRIST 2
00007 #define ARMPIT 3
00008 DigitalOut nReset(dp26);
00009 #if 0
00010 DigitalOut debugLED1(dp24);
00011 DigitalOut debugLED2(dp18);
00012 #else 
00013 DigitalOut debugLED1(dp1);
00014 DigitalOut debugLED2(dp2);
00015 #endif
00016 Timer timer;
00017 class AnalogInLPF;
00018 class AnalogInLPF : public AnalogIn
00019 {
00020     private:
00021     float alpha;
00022     float prevAnalog;
00023     float nowAnalog;
00024     public : AnalogInLPF(PinName pin,float alpha_ = 0.1) : AnalogIn(pin)
00025     {
00026         alpha = alpha_;
00027         prevAnalog = 0.0;
00028     } 
00029     float read(){
00030         nowAnalog = AnalogIn::read();
00031         nowAnalog = nowAnalog*alpha + (1-alpha)*prevAnalog;
00032         prevAnalog = nowAnalog;
00033         return nowAnalog;
00034     }
00035     short read_u16(){
00036         nowAnalog = AnalogIn::read();
00037         nowAnalog = nowAnalog*alpha + (1-alpha)*prevAnalog;
00038         prevAnalog = nowAnalog;
00039         return short(nowAnalog*0xFFFF);
00040     }
00041 };
00042 uint16_t map(uint16_t in, uint16_t inMin, uint16_t inMax, uint16_t outMin, uint16_t outMax);
00043 AnalogIn ArmSense[4] = {AnalogIn(dp10),AnalogIn(dp9),AnalogIn(dp11),AnalogIn(dp13)};
00044 void waitTime(float ti){
00045     Timer t;
00046     t.start();
00047     while(ti > t.read());
00048     t.stop();
00049     return;
00050 }
00051 //AnalogInLPF ArmSense[4] = {AnalogInLPF(dp10),AnalogInLPF(dp9),AnalogInLPF(dp11),AnalogInLPF(dp13)};
00052 Serial dev(dp16,dp15);
00053 #define dataNum 12
00054 union floatInByte
00055 {
00056     uint16_t si;
00057     unsigned char c[2];
00058 };
00059 char *tmp;
00060 void RX(){
00061     if(dev.getc() == 'L'){
00062         timer.reset();
00063     }
00064 }
00065 void reset(){
00066     nReset = 0;
00067     waitTime(0.001);
00068     nReset = 1;
00069 }
00070 int main() {
00071     nReset = true;
00072     tmp = new char[dataNum];
00073     debugLED1 = 1;
00074     debugLED2 = 0;
00075     for(int i = 0 ; i < 50; i++){
00076         debugLED2 = !debugLED2;
00077         waitTime(0.1);
00078     }
00079     dev.baud(115200);
00080     dev.attach(RX,Serial::RxIrq);
00081     timer.start();
00082     while(1) {
00083         //送信データ格納
00084         debugLED1 = 1;
00085         tmp[0] = '0';
00086         tmp[1] = 0;
00087         tmp[2] = 0;
00088         tmp[3] = 0;
00089         for(int i = 0 ;i < 4 ; i++){
00090             uint16_t in = ArmSense[i].read_u16();
00091             floatInByte temp;
00092             temp.si = in;
00093             tmp[4 + i*2] = temp.c[0];//マスター片腕
00094             tmp[5 + i*2] = temp.c[1];   //マスター片腕
00095         }
00096         //送信データを送る
00097         char *SerialData = tmp;
00098         for(int i = 0 ; i < dataNum ; i++){
00099             dev.putc(SerialData[i]);
00100         }
00101         while(timer.read_ms() > 1000){
00102             if(timer.read_ms() > 3000){
00103                 reset();
00104                 break;
00105             }
00106         }
00107         debugLED1 = 0;
00108         wait_ms(15);
00109         //delete SerialData;
00110     }
00111     
00112     delete tmp;
00113 }
00114 uint16_t map(uint16_t in, uint16_t inMin, uint16_t inMax, uint16_t outMin, uint16_t outMax) {
00115   // check it's within the range
00116   if (inMin<inMax) { 
00117     if (in <= inMin) 
00118       return outMin;
00119     if (in >= inMax)
00120       return outMax;
00121   } else {  // cope with input range being backwards.
00122     if (in >= inMin) 
00123       return outMin;
00124     if (in <= inMax)
00125       return outMax;
00126   }
00127   // calculate how far into the range we are
00128   float scale = float(in-inMin)/float(inMax-inMin);
00129   // calculate the output.
00130   return uint16_t(outMin + scale*(outMax-outMin));
00131 }