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.
Fork of MyLib by
Revision 11:d50a0e730654, committed 2017-10-25
- Comitter:
- kikoaac
- Date:
- Wed Oct 25 10:57:13 2017 +0000
- Parent:
- 10:d0b1160ee5c2
- Commit message:
- ??;
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Defines/defines.h Wed Oct 25 10:57:13 2017 +0000 @@ -0,0 +1,49 @@ +#include "mbed.h" +#define R 0 +#define L 1 +#define DEBUG 0 +#define DEBUG_R 1 +#define CALIB 0 +#define SetupCMD 'A' +#define Respons 'S' +#define dataNum 12+3 + +#define NOTINPUT -1 +#define LOADMOTION -3 +#define TIMEOUT -2 + +#if !CALIB +//1号機 +//uint16_t MinimumRangeR[4] = {4000,40000,53500,16000}; +//uint16_t MaxmumRangeR[4] = {55000,51000,58900,45000}; +//uint16_t MinimumRangeL[4] = {31500,8000,30000,19800}; +//uint16_t MaxmumRangeL[4] = {48000,46400,46200,45000}; +//2号機 + +uint16_t MinimumRangeR[4] = {11900,32600,52100,0xffff-37800}; +uint16_t MaxmumRangeR[4] = {54500,48000,56000,0xffff-8900}; +uint16_t MinimumRangeL[4] = {62900,11300,37700,14200}; +uint16_t MaxmumRangeL[4] = {45800,56000,27900,44000}; +#endif +#if CALIB +uint16_t MinimumRangeR[4] = {0,0,0,0}; +uint16_t MaxmumRangeR[4] = {0xffff,0xffff,0xffff,0xffff}; +uint16_t MinimumRangeL[4] = {0,0,0,0}; +uint16_t MaxmumRangeL[4] = {0xffff,0xffff,0xffff,0xffff}; +#endif + +union floatInByte +{ + uint16_t si; + unsigned char c[2]; +}; +#define BIT(n) (1 << n) +union charInBool +{ + bool Bool[8]; + uint8_t data; +}; + +uint8_t RXData[dataNum] = {'0'}; +Nunchuck ctrl(D4,D5); +int8_t accData[2][3];
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Defines/param_motion.h Wed Oct 25 10:57:13 2017 +0000 @@ -0,0 +1,17 @@ +#include "mbed.h" +#define motionNum 2 +uint8_t motion[motionNum][10] = {{ + 0b01110101,0b01110101,0b01110101,0b01110101,0b01110101, + 0b01110101,0b01110101,0b01110101,0b01110101,0b01110101}, + {0b10101011,0b10101011,0b10101011,0b10101011,0b10101011, + 0b10101011,0b10101011,0b10101011,0b10101011,0b10101011}}; + +#define playMotionFlame 3 +uint8_t PlayMotion[motionNum][playMotionFlame][2][12] = + { + { + {{'H',0,0,0,0,0,0,0,0,0,0},{'0',0,0,0,0,0,0,0,0,0,0}}, + {{'H',0,0,0,0,0,0,0,0,0,0},{'0',0,0,0,0,0,0,0,0,0,0}}, + {{'H',0,0,0,0,0,0,0,0,0,0},{'0',0,0,0,0,0,0,0,0,0,0}} + } + };
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Methods/methods.h Wed Oct 25 10:57:13 2017 +0000 @@ -0,0 +1,314 @@ +#include "mbed.h" +void RX(); +class AnalogInLPF : public AnalogIn +{ + private: + float alpha; + float prevAnalog; + float nowAnalog; + public : AnalogInLPF(PinName pin,float alpha_) : 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); + } +}; +class InLPF +{ + private: + float alpha; + float prevAnalog; + float nowAnalog; + public : InLPF(float alpha_ = 0.2) + { + alpha = alpha_; + prevAnalog = 0.0; + } + float read(float in){ + nowAnalog = in; + nowAnalog = nowAnalog*alpha + (1-alpha)*prevAnalog; + prevAnalog = nowAnalog; + return nowAnalog; + } + short read_u16(float in){ + nowAnalog = in; + nowAnalog = nowAnalog*alpha + (1-alpha)*prevAnalog; + prevAnalog = nowAnalog; + return short(nowAnalog*0xFFFF); + } +}; +class InHPF +{ + private: + float alpha; + float prevAnalog; + float nowAnalog; + public:float originAnalog; + public : InHPF(float alpha_ = 0.2) + { + alpha = alpha_; + prevAnalog = 0.0; + } + float read(float in){ + originAnalog = in; + nowAnalog = originAnalog; + nowAnalog = nowAnalog*alpha + (1-alpha)*prevAnalog; + prevAnalog = nowAnalog; + return originAnalog - nowAnalog; + } + short read_u16(float in){ + originAnalog = in; + nowAnalog = originAnalog; + nowAnalog = nowAnalog*alpha + (1-alpha)*prevAnalog; + prevAnalog = nowAnalog; + return short(originAnalog*0xffff - nowAnalog*0xFFFF); + } + float read(){ + return originAnalog - nowAnalog; + } + short read_u16(){ + return short(originAnalog*0xFFFF - nowAnalog*0xFFFF); + } + +}; +class AccHPF +{ + private: + float alpha; + float prevAnalog; + float nowAnalog; + public:float originAnalog; + public : AccHPF(float alpha_ = 0.2) + { + alpha = alpha_; + prevAnalog = 0.0; + } + float read(float in){ + originAnalog = in; + nowAnalog = originAnalog; + nowAnalog = nowAnalog*alpha + (1-alpha)*prevAnalog; + prevAnalog = nowAnalog; + return originAnalog - nowAnalog; + } + float read(){ + return originAnalog - nowAnalog; + } +}; +#define LP 1 +AnalogInLPF ArmSense[4] = {AnalogInLPF(A6,LP),AnalogInLPF(A5,LP),AnalogInLPF(A4,LP),AnalogInLPF(A3,LP)}; +InLPF ArmSense2[4] = {InLPF(LP),InLPF(LP),InLPF(LP),InLPF(LP)}; + +InHPF ArmInputSense[2][4] = {{InHPF(0.2),InHPF(0.2),InHPF(0.2),InHPF(0.2)} , {InHPF(0.01),InHPF(0.005),InHPF(0.01),InHPF(0.01)}}; +AccHPF accSense[2][3] = {{AccHPF(0.2),AccHPF(0.2),AccHPF(0.2)} , {AccHPF(0.2),AccHPF(0.2),AccHPF(0.2)}}; +int8_t map8(int8_t in, int8_t inMin, int8_t inMax, int8_t outMin, int8_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 int8_t(outMin + scale*float(outMax-outMin)); + } +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*float(outMax-outMin)); +} + +void waitTime(float ti){ + Timer t; + t.start(); + while(ti > t.read()); + t.stop(); + return; +} +int8_t getLAccx(){ + return (int8_t)RXData[12]; +} +int8_t getLAccy(){ + return (int8_t)RXData[13]; +} +int8_t getLAccz(){ + return (int8_t)RXData[14]; +} +int8_t getRAccx(){ + return (int8_t)ctrl.accx(); +} +int8_t getRAccy(){ + return (int8_t)ctrl.accy(); +} +int8_t getRAccz(){ + return (int8_t)ctrl.accz(); +} +int8_t (*getAcc[2][3])(void) = {{getRAccx,getRAccy,getRAccz},{getLAccx,getLAccy,getLAccz}}; +bool motionTriggerFlag = false; +int motionCount[motionNum] = {0}; +Timer motionTimer; +int loopCount = 0; +void initMotion(){ + motionTimer.reset(); + loopCount = 0; + for(int i = 0 ; i < motionNum ; i++)motionCount[i] = 0; + motionTriggerFlag = false; + +} +int MotionSense(){ + charInBool data; + charInBool data1; + //加速度データを取得する。 + for(int i = 0 ; i < 2 ; i ++ ){ + for(int j = 0 ; j < 3; j++){ + accData[i][j] = getAcc[i][j](); + accSense[i][j].read(float(accData[i][j])/0xff); + } + } + //モーション認識中にモーション入力がないときTIMEOUTを返す。 + if(motionTimer.read() > 0.2F && motionTriggerFlag == true){ + initMotion(); + return TIMEOUT; + } + //登録されたモーションと類似しているモーションIDを返す。 + #define max(a, b) ((a) > (b) ? (a) : (b)) + if(loopCount == 10){ + int max_motionCount=0; + int ID = NOTINPUT; + for(int i = 0 ; i < motionNum ; i ++){ + max_motionCount = max(max_motionCount,motionCount[i]); + if(max_motionCount == motionCount[i])ID = i; + } + initMotion(); + return ID; + } + + //加速度が一定の強さを越えるとモーション認識を開始する。 + if(motionTriggerFlag == false){ + bool f = false; + for(int i = 0 ; i < 2 ; i ++ ){ + for(int j = 0 ; j < 3; j++){ + f = f | bool(accSense[i][j].read() > 0.3F); + } + } + + if(!f)return NOTINPUT; + motionTriggerFlag = true; + for(int i = 0 ; i < motionNum ; i++)motionCount[i] = 0; + } + //可変抵抗の値を取得 + for(int i = 0; i < 2; i++ ){ + for (int j = 0 ; j < 4 ; j ++){ + data.data = (data.data<<1)|bool(fabs(ArmInputSense[i][j].read()) > 0.1F); + data1.data = (data1.data<<1)|bool(ArmInputSense[i][j].originAnalog > 0.5F); + } + } + /*sbdbt.printf("\n"); + for(int i = 7 ; i >= 0 ; i--) + sbdbt.printf("%d",(data.data>>i)&1); + sbdbt.printf("\n"); + for(int i = 7 ; i >= 0 ; i--) + sbdbt.printf("%d",(data1.data>>i)&1); + sbdbt.printf("\n");*/ + //可変抵抗に動きがあるかを判定 + if(data.data != 0){ + //可変抵抗の値と全モーションデータの比較 + for(int i = 0 ; i < motionNum ; i++){ + char d = data1.data & motion[i][loopCount]; + for(int j = 0 ; j < 8 ; j++){ + motionCount[i] += bool((d>>j)&1); + } + //等しければモーションカウントを++ + sbdbt.printf("motionCount[%d] %d",i,motionCount[i]); + } + //全体ループを++ + loopCount ++; + //タイマーリセット + motionTimer.reset(); + //モーション認識中なのでLOADMOTIONを返す。 + return LOADMOTION; + } + return NOTINPUT; +} +int playMotionCount = 0; +bool playMotionFlag = false; +void playMotion(int id,uint8_t** copy){ + if(playMotionFlag == false && playMotionCount == 0 ){ + playMotionFlag = true; + } + + for(int j = 0; j < 2 ; j++){ + for(int i = 0 ; i < dataNum-3 ; i++){ + for(int s = 0; s < 2; s++) + { + for(int m = 0 ; m < 12 ; m ++)copy[s][m] = PlayMotion[id][playMotionCount][s][m]; + } + + } + } + // if(!DEBUG)sbdbt.printf("\n"); + // dev.putc('L'); + // waitTime(0.01); + // } + // } + playMotionCount ++; + if(playMotionCount == playMotionFlame){ + playMotionCount = 0; + playMotionFlag = false; + } +} +float prevR=0; +int impulseCount = 0; +int impulseFlag = false; +bool impulse(double y){ + //if(sieta1 > PI/3 && sieta1 < PI*2/3.0) + float r = y; + float temp = r; + r = r*0.1F + (1-0.1F)*prevR; + prevR = r; + //sbdbt.printf(" %f",temp - r); + if(impulseFlag == true)impulseCount++; + if((temp - r) > 0.6F && impulseFlag == false){ + impulseFlag = true; + } + if((temp - r) < -0.1F && impulseCount <= 5 && impulseFlag == true){ + impulseCount = 0; + impulseFlag = false; + return true; + } + if(impulseCount > 6 && impulseFlag == true){ + impulseCount = 0; + impulseFlag = false; + } + return false; +} \ No newline at end of file
--- a/Nunchuck/Nunchuck.cpp Thu May 11 12:55:05 2017 +0000 +++ b/Nunchuck/Nunchuck.cpp Wed Oct 25 10:57:13 2017 +0000 @@ -107,30 +107,30 @@ } -int Nunchuck::accx() +int8_t Nunchuck::accx() { - getdata(); - int temp = data[2] << 2; + //getdata(); + int8_t temp = data[2] << 2; if ((data[5] >> 2) & 1) temp += 2; if ((data[5] >> 3) & 1) temp += 1; return temp; } -int Nunchuck::accy() +int8_t Nunchuck::accy() { - getdata(); - int temp = data[3] << 2; + //getdata(); + int8_t temp = data[3] << 2; if ((data[5] >> 4) & 1) temp += 2; if ((data[5] >> 5) & 1) temp += 1; return temp; } -int Nunchuck::accz() +int8_t Nunchuck::accz() { - getdata(); - int temp = data[4] << 2; + //getdata(); + int8_t temp = data[4] << 2; if ((data[5] >> 6) & 1) temp += 2; if ((data[5] >> 7) & 1) temp += 1; return temp;
--- a/Nunchuck/Nunchuck.h Thu May 11 12:55:05 2017 +0000 +++ b/Nunchuck/Nunchuck.h Wed Oct 25 10:57:13 2017 +0000 @@ -5,7 +5,7 @@ #include "mbed.h" #define NUNCHUCK_ANALOGDATA 1 //1 : analog 0 : degital -#define NUNCHUCK_DEADZONE 6 //analog stick's deadzone +#define NUNCHUCK_DEADZONE 0 //analog stick's deadzone #define NUNCHUCK_ADDR 0xA4 // 0x52 << 1 #define PI 3.14159265358979 @@ -20,18 +20,18 @@ double analograd(); double analogdeg(); double analogrange(); - int accx(); - int accy(); - int accz (); + int8_t accx(); + int8_t accy(); + int8_t accz(); bool buttonc(); bool buttonz(); + void getdata(); private: Timer timer; bool flag; bool init(); char data[6]; - void getdata(); }; #endif \ No newline at end of file
--- a/PS3/PS3.cpp Thu May 11 12:55:05 2017 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,143 +0,0 @@ -#include "PS3.h" - - PS3::PS3(PinName TX, PinName RX) : Serial(TX, RX) - { - PS3Data[0] = 128; - PS3Data[1] = 0; - PS3Data[2] = 0; - PS3Data[3] = 64; - PS3Data[4] = 64; - PS3Data[5] = 64; - PS3Data[6] = 64; - PS3Data[7] = 0; - baud(2400); - Serial::attach(this, &PS3::getdata, Serial::RxIrq); - } - - bool PS3::maru() - { - return PS3Data[2] & MARU; - } - - bool PS3::batu() - { - return PS3Data[2] & BATU; - } - - bool PS3::sikaku() - { - return PS3Data[1] & SIKAKU; - } - - bool PS3::sankaku() - { - return PS3Data[2] & SANKAKU; - } - - bool PS3::ue() - { - return PS3Data[2] & UE && !(PS3Data[2] & SITA); - } - - bool PS3::sita() - { - return PS3Data[2] & SITA && !(PS3Data[2] & UE); - } - - bool PS3::start() - { - return PS3Data[2] & UE && PS3Data[2] & SITA; - } - - bool PS3::migi() - { - return PS3Data[2] & MIGI && !(PS3Data[2] & HIDARI); - } - - bool PS3::hidari() - { - return PS3Data[2] & HIDARI && !(PS3Data[2] & MIGI); - } - - bool PS3::select() - { - return PS3Data[2] & MIGI && PS3Data[2] & HIDARI; - } - - bool PS3::L1() - { - return PS3Data[1] & LEFT1; - } - - bool PS3::L2() - { - return PS3Data[1] & LEFT2; - } - - bool PS3::R1() - { - return PS3Data[1] & RIGHT1; - } - - bool PS3::R2() - { - return PS3Data[1] & RIGHT2; - } - - int8_t PS3::analogLX() - { - if(PS3Data[3] == 0) - PS3Data[3] = 1; - return PS3Data[3] - 64; - } - - int8_t PS3::analogLY() - { - if(PS3Data[4] == 0) - PS3Data[4] = 1; - return (PS3Data[4] - 64) * (-1); - } - - int8_t PS3::analogRX() - { - if(PS3Data[5] == 0) - PS3Data[5] = 1; - return PS3Data[5] - 64; - } - - int8_t PS3::analogRY() - { - if(PS3Data[6] == 0) - PS3Data[6] = 1; - return (PS3Data[6] - 64) * (-1); - } - - - - void PS3::getdata() - { - while(Serial::getc() != 128) - { - } - for(int i = 1; i < 8; i++) - { - GetData[i] = Serial::getc(); - } - - sum = GetData[1] + GetData[2]; - for(int i = 3; i < 7; i++) - { - sum += GetData[i] - 64; - } - - if(sum < 0) - sum += 128; - - if(sum == GetData[7]) - { - for(int i = 0; i < 8; i++) - { - PS3Data[i] = GetData[i]; - } - } - } \ No newline at end of file
--- a/PS3/PS3.h Thu May 11 12:55:05 2017 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,173 +0,0 @@ -#ifndef PS3_H -#define PS3_H -#include "mbed.h" - -#define MARU 64 -#define BATU 32 -#define SIKAKU 1 -#define SANKAKU 16 -#define UE 1 -#define SITA 2 -#define MIGI 4 -#define HIDARI 8 -#define LEFT1 2 -#define LEFT2 4 -#define RIGHT1 8 -#define RIGHT2 16 - -class PS3 : public Serial -{ - public: - PS3(PinName TX, PinName RX); - /*{ - PS3Data[0] = 128; - PS3Data[1] = 0; - PS3Data[2] = 0; - PS3Data[3] = 64; - PS3Data[4] = 64; - PS3Data[5] = 64; - PS3Data[6] = 64; - PS3Data[7] = 0; - baud(2400); - Serial::attach(this, &PS3::getdata, Serial::RxIrq); - }*/ - - bool maru(); - /*{ - return PS3Data[2] & MARU; - }*/ - - bool batu(); - /*{ - return PS3Data[2] & BATU; - }*/ - - bool sikaku(); - /*{ - return PS3Data[1] & SIKAKU; - }*/ - - bool sankaku(); - /*{ - return PS3Data[2] & SANKAKU; - }*/ - - bool ue(); - /*{ - return PS3Data[2] & UE && !(PS3Data[2] & SITA); - }*/ - - bool sita(); - /*{ - return PS3Data[2] & SITA && !(PS3Data[2] & UE); - }*/ - - bool start(); - /*{ - return PS3Data[2] & UE && PS3Data[2] & SITA; - }*/ - - bool migi(); - /*{ - return PS3Data[2] & MIGI && !(PS3Data[2] & HIDARI); - }*/ - - bool hidari(); - /*{ - return PS3Data[2] & HIDARI && !(PS3Data[2] & MIGI); - }*/ - - bool select(); - /*{ - return PS3Data[2] & MIGI && PS3Data[2] & HIDARI; - }*/ - - bool L1(); - /*{ - return PS3Data[1] & LEFT1; - }*/ - - bool L2(); - /*{ - return PS3Data[1] & LEFT2; - }*/ - - bool R1(); - /*{ - return PS3Data[1] & RIGHT1; - }*/ - - bool R2(); - /*{ - return PS3Data[1] & RIGHT2; - }*/ - - int8_t analogLX(); - /*{ - if(PS3Data[3] == 0) - PS3Data[3]=1; - return PS3Data[3]-64; - }*/ - - int8_t analogLY(); - /*{ - if(PS3Data[4] == 0) - PS3Data[4]=1; - return (PS3Data[4]-64)*(-1); - }*/ - - int8_t analogRX(); - /*{ - if(PS3Data[5] == 0) - PS3Data[5]=1; - return PS3Data[5]-64; - }*/ - - int8_t analogRY(); - /*{ - if(PS3Data[6] == 0) - PS3Data[6]=1; - return (PS3Data[6]-64)*(-1); - }*/ - - - - private: - - int8_t sum; - uint8_t PS3Data[8]; - uint8_t GetData[8]; - - void getdata(); - /*{ - while(Serial::getc() != 128) - { - } - for(int i = 1;i < 8;i++) - { - GetData[i] = Serial::getc(); - } - - sum = GetData[1] + GetData[2]; - for(int i = 3;i < 7;i++) - { - sum += GetData[i] - 64; - } - - if(sum < 0) - sum+=128; - - if(sum == GetData[7]) - { - for(int i = 0;i < 8;i++) - { - PS3Data[i]=GetData[i]; - } - } - }*/ - -}; - -#endif - - \ No newline at end of file