Kotaro Yamamoto / QMotor

おためしらいぶらり　Q-rover-Kaiを50行以内で動かすという強い気持ち　使用非推奨　動作未確認　単純なPWM動作のライブラリ化実験用

Fork of QMotor by Kotaro Yamamoto

QMotor.cpp

Committer:
KOTAROYamamoto
Date:
2017-12-06
Revision:
1:b80cd8133481
Parent:
0:48d84c228105

File content as of revision 1:b80cd8133481:

#include "mbed.h"
#include "QMotor.h"

QMotor::QMotor(PinName R1,PinName R2,PinName L1,PinName L2):_R1(R1),_R2(R2),_L1(L1),_L2(L2){
    _R1=0.0;//R1を初期化
    _R2=0.0;//R2を初期化
    _L1=0.0;//L1を初期化
    _L2=0.0;//L2を初期化
    float san1;
    float san2;
}

void QMotor::Front1(){                   //直進部
    _R1=1.0;
    _R2=0.0;
    _L1=1.0;
    _L2=0.0;
    wait(25.000);
        
    _R1=1.0;
    _R2=1.0;
    _L1=1.0;
    _L2=1.0;
    wait(0.200);
    _R1=0.0;
    _R2=0.0;
    _L1=0.0;
    _L2=0.0;
}

void QMotor::Front2(){                   //直進部
    _R1=0.5;
    _R2=0.0;
    _L1=0.5;
    _L2=0.0;
    wait(5.000);
        
    _R1=1.0;
    _R2=1.0;
    _L1=1.0;
    _L2=1.0;
    wait(0.200);
    _R1=0.0;
    _R2=0.0;
    _L1=0.0;
    _L2=0.0;
}
void QMotor::Right1(float an){          //第一while関数右折部
    //sdprint(" an=");
    //sdprint(an);
    //sdprint(" R1 ");
    //xbee.printf("COMMAND:Right1\n");
    _R1=1.0;
    _R2=0.0;
    wait(-an*san1);
        
    _R1=1.0;
    _R2=1.0;
    wait(0.200);
    _R1=0.0;
    _R2=0.0;
  }
  
  void QMotor::Right2(float an){          //第二while関数右折部
    //sdprint(" an=");
    //sdprint(an);
    //sdprint(" R2 ");
    //xbee.printf("COMMAND:Right2\n");
    //analogWrite(5,76.5);//Lモーターを30%出力で回転
    _R1=1.0;
    _R2=0.0;
    wait(-an*san2);
        
    _R1=1.0;
    _R2=1.0;
    wait(0.200);
    _R1=0.0;
    _R2=0.0;
  }
  
  
  void QMotor::Left1(float an){           //第一while関数左折部
    //sdprint(" an=");
    //sdprint(an);
    //sdprint(" L1 ");
    //xbee.printf("COMMAND:Left1\n");
    _L1=1.0;
    _L2=0.0;
    wait(an*san1);
        
    _L1=1.0;
    _L2=1.0;
    wait(0.200);
    _L1=0.0;
    _L2=0.0;
  }

  void QMotor::Left2(float an){           //第二while関数左折部
    //sdprint(" an=");
    //sdprint(an);
    //sdprint(" L2 ");
    //xbee.printf("COMMAND:Left2\n");
    //analogWrite(6,76.5);//Rモーターを30%出力で回転
    _L1=1.0;
    _L2=0.0;
    wait(an*san2);
        
    _L1=1.0;
    _L2=1.0;
    wait(0.200);
    _L1=0.0;
    _L2=0.0;      
  }
  
void QMotor::Reverse(){                 //姿勢回復機動
    _R1=0.0;
    _R2=1.0;
    _L1=0.0;
    _L2=1.0;
    wait(15.000);
    _R1=1.0;
    _R2=0.0;
    _L1=1.0;
    _L2=0.0;
    wait(5.000);
    _R1=0.0;
    _R2=0.0;
    _L1=0.0;
    _L2=0.0;
}
  
void QMotor::Stack(){                   //スタック回避挙動
    _R1=1.0;
    _R2=0.0;
    _L1=0.0;
    _L2=1.0;
    wait(5.000);

    _R1=1.0;
    _R2=1.0;
    _L1=1.0;
    _L2=1.0;
    wait(5.000);
    _L1=0.0;
    _L2=0.0;
    _R1=0.0;
    _R2=0.0;
  }
  
    void QMotor::Fred(){                    //冗長系測角用直進部
    _R1=1.0;
    _R2=0.0;
    _L1=1.0;
    _L2=0.0;
    wait(10.000);
      
    _R1=1.0;
    _R2=1.0;
    _L1=1.0;
    _L2=1.0;
    wait(0.200);
    _R1=0.0;
    _R2=0.0;
    _L1=0.0;
    _L2=0.0;
  }