Diff: main.cpp

Revision:

0:038c12ba8a60

Child:

1:99a635cd2f19

diff -r 000000000000 -r 038c12ba8a60 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Oct 13 23:01:29 2017 +0000
@@ -0,0 +1,121 @@
+#include "mbed.h"
+#include "Nunchuck.h"
+#define R 0
+#define L 1
+#define SHOULDER 0
+#define ELBOW 1
+#define WRIST 2
+#define ARMPIT 3
+#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)};
+
+//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();
+    }
+}
+int main() {
+    tmp = new char[dataNum];
+    debugLED1 = 1;
+    debugLED2 = 0;
+    for(int i = 0 ; i < 50; i++){
+        debugLED2 = !debugLED2;
+        wait(0.1);
+    }
+    dev.baud(115200);
+    dev.attach(RX,Serial::RxIrq);
+    timer.start();
+    while(1) {
+        //送信データ格納
+            
+        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() > 300){
+            debugLED2 = true;
+            wait(0.5);
+            debugLED2 = false;
+            wait(0.5);
+        }
+        debugLED1 = 0;
+        wait_ms(1);
+        //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));
+}
\ No newline at end of file