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@1:99a635cd2f19, 2017-10-24 (annotated)

Committer:
kikoaac
Date:
Tue Oct 24 15:37:15 2017 +0000
Revision:
1:99a635cd2f19
Parent:
0:038c12ba8a60
Child:
2:22aa8109e2f0
????????

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 1:99a635cd2f19 9 DigitalOut nReset(dp26);
kikoaac 0:038c12ba8a60 10 #if 0
kikoaac 0:038c12ba8a60 11 DigitalOut debugLED1(dp24);
kikoaac 0:038c12ba8a60 12 DigitalOut debugLED2(dp18);
kikoaac 0:038c12ba8a60 13 #else
kikoaac 0:038c12ba8a60 14 DigitalOut debugLED1(dp1);
kikoaac 0:038c12ba8a60 15 DigitalOut debugLED2(dp2);
kikoaac 0:038c12ba8a60 16 #endif
kikoaac 0:038c12ba8a60 17 Timer timer;
kikoaac 0:038c12ba8a60 18 class AnalogInLPF;
kikoaac 0:038c12ba8a60 19 class AnalogInLPF : public AnalogIn
kikoaac 0:038c12ba8a60 20 {
kikoaac 0:038c12ba8a60 21 private:
kikoaac 0:038c12ba8a60 22 float alpha;
kikoaac 0:038c12ba8a60 23 float prevAnalog;
kikoaac 0:038c12ba8a60 24 float nowAnalog;
kikoaac 0:038c12ba8a60 25 public : AnalogInLPF(PinName pin,float alpha_ = 0.1) : AnalogIn(pin)
kikoaac 0:038c12ba8a60 26 {
kikoaac 0:038c12ba8a60 27 alpha = alpha_;
kikoaac 0:038c12ba8a60 28 prevAnalog = 0.0;
kikoaac 0:038c12ba8a60 29 }
kikoaac 0:038c12ba8a60 30 float read(){
kikoaac 0:038c12ba8a60 31 nowAnalog = AnalogIn::read();
kikoaac 0:038c12ba8a60 32 nowAnalog = nowAnalog*alpha + (1-alpha)*prevAnalog;
kikoaac 0:038c12ba8a60 33 prevAnalog = nowAnalog;
kikoaac 0:038c12ba8a60 34 return nowAnalog;
kikoaac 0:038c12ba8a60 35 }
kikoaac 0:038c12ba8a60 36 short read_u16(){
kikoaac 0:038c12ba8a60 37 nowAnalog = AnalogIn::read();
kikoaac 0:038c12ba8a60 38 nowAnalog = nowAnalog*alpha + (1-alpha)*prevAnalog;
kikoaac 0:038c12ba8a60 39 prevAnalog = nowAnalog;
kikoaac 0:038c12ba8a60 40 return short(nowAnalog*0xFFFF);
kikoaac 0:038c12ba8a60 41 }
kikoaac 0:038c12ba8a60 42 };
kikoaac 0:038c12ba8a60 43 uint16_t map(uint16_t in, uint16_t inMin, uint16_t inMax, uint16_t outMin, uint16_t outMax);
kikoaac 0:038c12ba8a60 44 AnalogIn ArmSense[4] = {AnalogIn(dp10),AnalogIn(dp9),AnalogIn(dp11),AnalogIn(dp13)};
kikoaac 1:99a635cd2f19 45 void waitTime(float ti){
kikoaac 1:99a635cd2f19 46 Timer t;
kikoaac 1:99a635cd2f19 47 t.start();
kikoaac 1:99a635cd2f19 48 while(ti > t.read());
kikoaac 1:99a635cd2f19 49 t.stop();
kikoaac 1:99a635cd2f19 50 return;
kikoaac 1:99a635cd2f19 51 }
kikoaac 0:038c12ba8a60 52 //AnalogInLPF ArmSense[4] = {AnalogInLPF(dp10),AnalogInLPF(dp9),AnalogInLPF(dp11),AnalogInLPF(dp13)};
kikoaac 0:038c12ba8a60 53 Nunchuck ctrl(dp5,dp27);
kikoaac 0:038c12ba8a60 54 Serial dev(dp16,dp15);
kikoaac 0:038c12ba8a60 55 #define dataNum 12
kikoaac 0:038c12ba8a60 56 union floatInByte
kikoaac 0:038c12ba8a60 57 {
kikoaac 0:038c12ba8a60 58 uint16_t si;
kikoaac 0:038c12ba8a60 59 unsigned char c[2];
kikoaac 0:038c12ba8a60 60 };
kikoaac 0:038c12ba8a60 61 char *tmp;
kikoaac 0:038c12ba8a60 62 void RX(){
kikoaac 0:038c12ba8a60 63 if(dev.getc() == 'L'){
kikoaac 0:038c12ba8a60 64 timer.reset();
kikoaac 0:038c12ba8a60 65 }
kikoaac 0:038c12ba8a60 66 }
kikoaac 1:99a635cd2f19 67 void reset(){
kikoaac 1:99a635cd2f19 68 nReset = 0;
kikoaac 1:99a635cd2f19 69 waitTime(0.001);
kikoaac 1:99a635cd2f19 70 nReset = 1;
kikoaac 1:99a635cd2f19 71 }
kikoaac 0:038c12ba8a60 72 int main() {
kikoaac 1:99a635cd2f19 73 nReset = true;
kikoaac 0:038c12ba8a60 74 tmp = new char[dataNum];
kikoaac 0:038c12ba8a60 75 debugLED1 = 1;
kikoaac 0:038c12ba8a60 76 debugLED2 = 0;
kikoaac 0:038c12ba8a60 77 for(int i = 0 ; i < 50; i++){
kikoaac 0:038c12ba8a60 78 debugLED2 = !debugLED2;
kikoaac 1:99a635cd2f19 79 waitTime(0.1);
kikoaac 0:038c12ba8a60 80 }
kikoaac 1:99a635cd2f19 81 ctrl.offset_();
kikoaac 0:038c12ba8a60 82 dev.baud(115200);
kikoaac 0:038c12ba8a60 83 dev.attach(RX,Serial::RxIrq);
kikoaac 0:038c12ba8a60 84 timer.start();
kikoaac 0:038c12ba8a60 85 while(1) {
kikoaac 0:038c12ba8a60 86 //送信データ格納
kikoaac 1:99a635cd2f19 87 ctrl.getdata();
kikoaac 0:038c12ba8a60 88 debugLED1 = 1;
kikoaac 0:038c12ba8a60 89 tmp[0] = '0';
kikoaac 0:038c12ba8a60 90 tmp[1] = ctrl.analogx();//ヌンチャクアナログX
kikoaac 0:038c12ba8a60 91 tmp[2] = ctrl.analogy();//ヌンチャクアナログy
kikoaac 0:038c12ba8a60 92 tmp[3] = (ctrl.buttonc()<<1)|(ctrl.buttonz());//ヌンチャクzボタンとCボタン
kikoaac 0:038c12ba8a60 93 for(int i = 0 ;i < 4 ; i++){
kikoaac 0:038c12ba8a60 94 uint16_t in = ArmSense[i].read_u16();
kikoaac 0:038c12ba8a60 95 floatInByte temp;
kikoaac 0:038c12ba8a60 96 temp.si = in;
kikoaac 0:038c12ba8a60 97 tmp[4 + i*2] = temp.c[0];//マスター片腕
kikoaac 0:038c12ba8a60 98 tmp[5 + i*2] = temp.c[1]; //マスター片腕
kikoaac 0:038c12ba8a60 99 }
kikoaac 0:038c12ba8a60 100 //送信データを送る
kikoaac 0:038c12ba8a60 101 char *SerialData = tmp;
kikoaac 0:038c12ba8a60 102 for(int i = 0 ; i < dataNum ; i++){
kikoaac 0:038c12ba8a60 103 dev.putc(SerialData[i]);
kikoaac 0:038c12ba8a60 104 }
kikoaac 1:99a635cd2f19 105 while(timer.read_ms() > 1000){
kikoaac 1:99a635cd2f19 106 double x = (int8_t)ctrl.analogx();//ヌンチャクアナログX
kikoaac 1:99a635cd2f19 107 double y = (int8_t)ctrl.analogy();//ヌンチャクアナログy
kikoaac 1:99a635cd2f19 108 bool b1 = ctrl.buttonc();
kikoaac 1:99a635cd2f19 109 bool b2 = ctrl.buttonz();//ヌンチャクzボタンとCボタン
kikoaac 1:99a635cd2f19 110 double sieta = atan2(x,y);
kikoaac 0:038c12ba8a60 111 debugLED2 = true;
kikoaac 1:99a635cd2f19 112 waitTime(0.5);
kikoaac 0:038c12ba8a60 113 debugLED2 = false;
kikoaac 1:99a635cd2f19 114 waitTime(0.5);
kikoaac 1:99a635cd2f19 115 if(sieta < 3.14/3.0 && sieta > -3.14/3.0 && b1 == true && b2 == true){
kikoaac 1:99a635cd2f19 116 reset();
kikoaac 1:99a635cd2f19 117 break;
kikoaac 1:99a635cd2f19 118 }
kikoaac 1:99a635cd2f19 119 if(timer.read_ms() > 1500){
kikoaac 1:99a635cd2f19 120 reset();
kikoaac 1:99a635cd2f19 121 break;
kikoaac 1:99a635cd2f19 122 }
kikoaac 0:038c12ba8a60 123 }
kikoaac 0:038c12ba8a60 124 debugLED1 = 0;
kikoaac 1:99a635cd2f19 125 wait_ms(15);
kikoaac 0:038c12ba8a60 126 //delete SerialData;
kikoaac 0:038c12ba8a60 127 }
kikoaac 0:038c12ba8a60 128
kikoaac 0:038c12ba8a60 129 delete tmp;
kikoaac 0:038c12ba8a60 130 }
kikoaac 0:038c12ba8a60 131 uint16_t map(uint16_t in, uint16_t inMin, uint16_t inMax, uint16_t outMin, uint16_t outMax) {
kikoaac 0:038c12ba8a60 132 // check it's within the range
kikoaac 0:038c12ba8a60 133 if (inMin<inMax) {
kikoaac 0:038c12ba8a60 134 if (in <= inMin)
kikoaac 0:038c12ba8a60 135 return outMin;
kikoaac 0:038c12ba8a60 136 if (in >= inMax)
kikoaac 0:038c12ba8a60 137 return outMax;
kikoaac 0:038c12ba8a60 138 } else { // cope with input range being backwards.
kikoaac 0:038c12ba8a60 139 if (in >= inMin)
kikoaac 0:038c12ba8a60 140 return outMin;
kikoaac 0:038c12ba8a60 141 if (in <= inMax)
kikoaac 0:038c12ba8a60 142 return outMax;
kikoaac 0:038c12ba8a60 143 }
kikoaac 0:038c12ba8a60 144 // calculate how far into the range we are
kikoaac 0:038c12ba8a60 145 float scale = float(in-inMin)/float(inMax-inMin);
kikoaac 0:038c12ba8a60 146 // calculate the output.
kikoaac 0:038c12ba8a60 147 return uint16_t(outMin + scale*(outMax-outMin));
kikoaac 0:038c12ba8a60 148 }