main.cpp

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