Kiko Ishimoto / Mbed 2 deprecated robocon2017mbed_control_R

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   MyLib2 mbed

main.cpp@0:038c12ba8a60, 2017-10-13 (annotated)

Committer:
kikoaac
Date:
Fri Oct 13 23:01:29 2017 +0000
Revision:
0:038c12ba8a60
Child:
1:99a635cd2f19
???????

Who changed what in which revision?

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