File content as of revision 1:99a635cd2f19:

#include "mbed.h"
#include "Nunchuck.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)};
Nunchuck ctrl(dp5,dp27);
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);
    }
    ctrl.offset_();
    dev.baud(115200);
    dev.attach(RX,Serial::RxIrq);
    timer.start();
    while(1) {
        //送信データ格納
        ctrl.getdata();
        debugLED1 = 1;
        tmp[0] = '0';
        tmp[1] = ctrl.analogx();//ヌンチャクアナログX
        tmp[2] = ctrl.analogy();//ヌンチャクアナログy
        tmp[3] = (ctrl.buttonc()<<1)|(ctrl.buttonz());//ヌンチャクzボタンとCボタン
        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){
            double x = (int8_t)ctrl.analogx();//ヌンチャクアナログX
            double y = (int8_t)ctrl.analogy();//ヌンチャクアナログy
            bool b1 = ctrl.buttonc();
            bool b2 = ctrl.buttonz();//ヌンチャクzボタンとCボタン
            double sieta = atan2(x,y);
            debugLED2 = true;
            waitTime(0.5);
            debugLED2 = false;
            waitTime(0.5);
            if(sieta < 3.14/3.0 && sieta > -3.14/3.0 && b1 == true && b2 == true){
                reset();
                break;
            }
            if(timer.read_ms() > 1500){
                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));
}