Kiko Ishimoto
うい
Fork of
MyLib
by 
gaku takasawa
Diff: Methods/methods.h
- Revision:
- 11:d50a0e730654
--- /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