Akira Minomo
Line Trace Robot Program Sagami
Diff: motor.cpp
- Revision:
- 0:95bb05165e79
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/motor.cpp Sun Oct 09 05:10:15 2016 +0000
@@ -0,0 +1,1462 @@
+/******************************************************************************
+ @file motor.cpp
+ @brief メカナムホイール用モーター制御
+ PWMによりメカナムホイールの回転数を設定し、
+ 進行方向に対して走行を制御する
+ @date 2015/11/14
+ @author Akira Minomo
+ @par History
+ - 2015/11/14 Akira Minomo
+ -# Initial Version
+******************************************************************************/
+
+#include "mbed.h"
+#include "motor.h"
+
+#define CHANGE_CROSS 1
+#define CHANGE_X 0
+
+
+PwmOut moterLF(p24);
+PwmOut moterRF(p23);
+PwmOut moterLB(p22);
+PwmOut moterRB(p21);
+
+PwmOut motorArm(p25);
+
+//センサーの動作ロジック
+int modeBefore=MODE_F;
+int mode=MODE_F;
+unsigned char mode_last2[8]={1,1,1,0,1,0,1,1};
+
+//アームを持ち上げた状態にする
+void ArmUp()
+{
+ motorArm.pulsewidth_us(ARM_SERVO_UP);
+}
+
+//アームを下げた状態にする(特産物をおろす)
+void ArmDown()
+{
+ motorArm.pulsewidth_us(ARM_SERVO_DOWN);
+}
+
+//ボーナス時の動作
+void BonusTime()
+{
+ moveStop();
+ wait(1);
+ ArmDown();
+ wait(1);
+ moveB();
+ wait(0.3);
+ moveStop();
+}
+
+//iビット目の値を取り出す。
+bool IsLine(unsigned char data ,int i)
+{
+ data=data>>i;
+
+ if((data & 0x01) && (mode_last2[i])) {
+ return true;
+ }else{
+ return false;
+ }
+}
+
+// 前回の進行方向をもとに変移の有効無効を切り替える
+void juModeEna(int mode) {
+ switch (mode) {
+ case MODE_F:
+ mode_last2[MODE_F] = 1;
+ mode_last2[MODE_RF] = 1;
+ mode_last2[MODE_R] = CHANGE_CROSS;
+ mode_last2[MODE_RB] = 1;//0
+ mode_last2[MODE_B] = 1;//0;
+ mode_last2[MODE_LB] = 1;//0
+ mode_last2[MODE_L] = CHANGE_CROSS;
+ mode_last2[MODE_LF] = 1;
+ break;
+ case MODE_RF:
+ mode_last2[MODE_F] = 1;
+ mode_last2[MODE_RF] = 1;
+ mode_last2[MODE_R] = 1;
+ mode_last2[MODE_RB] = CHANGE_X;
+ mode_last2[MODE_B] = 0;//0
+ mode_last2[MODE_LB] = 0;//0;
+ mode_last2[MODE_L] = 0;//0
+ mode_last2[MODE_LF] = CHANGE_X;
+ break;
+ case MODE_R:
+ mode_last2[MODE_F] = CHANGE_CROSS;
+ mode_last2[MODE_RF] = 1;
+ mode_last2[MODE_R] = 1;
+ mode_last2[MODE_RB] = 1;//ダメな場合はここを変更する
+ mode_last2[MODE_B] = 0;//CHANGE_CROSS;
+ mode_last2[MODE_LB] = 0;
+ mode_last2[MODE_L] = 0;
+ mode_last2[MODE_LF] = 0;
+ break;
+ case MODE_RB:
+ mode_last2[MODE_F] = 0;
+ mode_last2[MODE_RF] = CHANGE_X;
+ mode_last2[MODE_R] = 1;
+ mode_last2[MODE_RB] = 1;
+ mode_last2[MODE_B] = 0;//1;
+ mode_last2[MODE_LB] = CHANGE_X;
+ mode_last2[MODE_L] = 0;
+ mode_last2[MODE_LF] = 0;
+ break;
+ case MODE_B:
+ mode_last2[MODE_F] = 0;
+ mode_last2[MODE_RF] = 0;
+ mode_last2[MODE_R] = CHANGE_CROSS;
+ mode_last2[MODE_RB] = 1;
+ mode_last2[MODE_B] = 1;
+ mode_last2[MODE_LB] = 1;
+ mode_last2[MODE_L] = CHANGE_CROSS;
+ mode_last2[MODE_LF] = 0;
+ break;
+ case MODE_LB:
+ mode_last2[MODE_F] = 0;
+ mode_last2[MODE_RF] = 0;
+ mode_last2[MODE_R] = 0;
+ mode_last2[MODE_RB] = CHANGE_X;
+ mode_last2[MODE_B] = 1;
+ mode_last2[MODE_LB] = 1;
+ mode_last2[MODE_L] = 1;
+ mode_last2[MODE_LF] = CHANGE_X;
+ break;
+ case MODE_L:
+ mode_last2[MODE_F] = 0;//CHANGE_CROSS;
+ mode_last2[MODE_RF] = 0;
+ mode_last2[MODE_R] = 0;
+ mode_last2[MODE_RB] = 0;
+ mode_last2[MODE_B] = 1;//CHANGE_CROSS;
+ mode_last2[MODE_LB] = 1;
+ mode_last2[MODE_L] = 1;
+ mode_last2[MODE_LF] = 1;
+ break;
+ case MODE_LF:
+ mode_last2[MODE_F] = 1;
+ mode_last2[MODE_RF] = CHANGE_X;
+ mode_last2[MODE_R] = 0;
+ mode_last2[MODE_RB] = 0;
+ mode_last2[MODE_B] = 0;
+ mode_last2[MODE_LB] = CHANGE_X;
+ mode_last2[MODE_L] = 1;
+ mode_last2[MODE_LF] = 1;
+ break;
+ default:
+ break;
+ }
+}
+/*
+int setMove(char cSensorData)
+{
+ static unsigned char ucGoalCnt = 0;
+ static unsigned char before_mode = 0;
+ static unsigned char change_before_mode = MODE_F;
+ static unsigned char ucBSensorData = 0xFF;
+ unsigned char ucGoalCheckFlag = FLAG_OFF;
+
+ //センサ入力に応じて進行方向を制御する
+ before_mode = mode;
+
+ if(mode == MODE_F){
+// if(change_before_mode==MODE_F ||change_before_mode==MODE_RF || change_before_mode==MODE_R){
+ if(change_before_mode==MODE_LF || change_before_mode==MODE_L){
+ if(IsLine(cSensorData,MODE_F)){
+ juModeEna(mode);
+ mode = MODE_F;
+ }else if(IsLine(cSensorData,MODE_RF)){
+ juModeEna(mode);
+ mode = MODE_RF;
+ }else if(IsLine(cSensorData,MODE_LF)){
+ juModeEna(mode);
+ mode = MODE_LF;
+ }else if(IsLine(cSensorData,MODE_R)){
+ juModeEna(mode);
+ mode = MODE_R;
+ }else if(IsLine(cSensorData,MODE_L)){
+ juModeEna(mode);
+ mode = MODE_L;
+ }else if(IsLine(cSensorData,MODE_B)){
+ juModeEna(mode);
+ mode = MODE_F;
+ ucGoalCheckFlag=FLAG_ON;
+ }else if(IsLine(cSensorData,MODE_RB)){
+ juModeEna(mode);
+ mode = MODE_F;
+ ucGoalCheckFlag=FLAG_ON;
+ }else if(IsLine(cSensorData,MODE_LB)){
+ juModeEna(mode);
+ mode = MODE_F;
+ ucGoalCheckFlag=FLAG_ON;
+ }
+ }else{
+ if(IsLine(cSensorData,MODE_F)){
+ juModeEna(mode);
+ mode = MODE_F;
+ }else if(IsLine(cSensorData,MODE_LF)){
+ juModeEna(mode);
+ mode = MODE_LF;
+ }else if(IsLine(cSensorData,MODE_RF)){
+ juModeEna(mode);
+ mode = MODE_RF;
+ }else if(IsLine(cSensorData,MODE_L)){
+ juModeEna(mode);
+ mode = MODE_L;
+ }else if(IsLine(cSensorData,MODE_R)){
+ juModeEna(mode);
+ mode = MODE_R;
+ }else if(IsLine(cSensorData,MODE_B)){
+ juModeEna(mode);
+ mode = MODE_F;
+ ucGoalCheckFlag=FLAG_ON;
+ }else if(IsLine(cSensorData,MODE_LB)){
+ juModeEna(mode);
+ mode = MODE_F;
+ ucGoalCheckFlag=FLAG_ON;
+ }else if(IsLine(cSensorData,MODE_RB)){
+ juModeEna(mode);
+ mode = MODE_F;
+ ucGoalCheckFlag=FLAG_ON;
+ }
+ }
+ }else if(mode == MODE_RF){
+ if(change_before_mode==MODE_RF ||change_before_mode==MODE_R || change_before_mode==MODE_RB){
+// if(change_before_mode==MODE_F ||change_before_mode==MODE_LF){
+ if(IsLine(cSensorData,MODE_RF)){
+ juModeEna(mode);
+ mode = MODE_RF;
+ }else if(IsLine(cSensorData,MODE_R)){
+ juModeEna(mode);
+ mode = MODE_R;
+ }else if(IsLine(cSensorData,MODE_RB)){
+ juModeEna(mode);
+ mode = MODE_RB;
+ }else if(IsLine(cSensorData,MODE_F)){
+ juModeEna(mode);
+ mode = MODE_F;
+ }else if(IsLine(cSensorData,MODE_LF)){
+ juModeEna(mode);
+ mode = MODE_LF;
+ }else if(IsLine(cSensorData,MODE_B)){
+ juModeEna(mode);
+ mode = MODE_RF;
+ ucGoalCheckFlag=FLAG_ON;
+ }else if(IsLine(cSensorData,MODE_L)){
+ juModeEna(mode);
+ mode = MODE_RF;
+ ucGoalCheckFlag=FLAG_ON;
+ }else if(IsLine(cSensorData,MODE_LB)){
+ juModeEna(mode);
+ mode = MODE_RF;
+ ucGoalCheckFlag=FLAG_ON;
+ }
+ }else{
+ if(IsLine(cSensorData,MODE_RF)){
+ juModeEna(mode);
+ mode = MODE_RF;
+ }else if(IsLine(cSensorData,MODE_F)){
+ juModeEna(mode);
+ mode = MODE_F;
+ }else if(IsLine(cSensorData,MODE_LF)){
+ juModeEna(mode);
+ mode = MODE_LF;
+ }else if(IsLine(cSensorData,MODE_R)){
+ juModeEna(mode);
+ mode = MODE_R;
+ }else if(IsLine(cSensorData,MODE_RB)){
+ juModeEna(mode);
+ mode = MODE_RB;
+ }else if(IsLine(cSensorData,MODE_L)){
+ juModeEna(mode);
+ mode = MODE_RF;
+ ucGoalCheckFlag=FLAG_ON;
+ }else if(IsLine(cSensorData,MODE_B)){
+ juModeEna(mode);
+ mode = MODE_RF;
+ ucGoalCheckFlag=FLAG_ON;
+ }else if(IsLine(cSensorData,MODE_LB)){
+ juModeEna(mode);
+ mode = MODE_RF;
+ ucGoalCheckFlag=FLAG_ON;
+ }
+ }
+ }else if(mode == MODE_R){
+ if(change_before_mode==MODE_R ||change_before_mode==MODE_RF || change_before_mode==MODE_F){
+// if(change_before_mode==MODE_RB ||change_before_mode==MODE_B){
+ if(IsLine(cSensorData,MODE_R)){
+ juModeEna(mode);
+ mode = MODE_R;
+ }else if(IsLine(cSensorData,MODE_RF)){
+ juModeEna(mode);
+ mode = MODE_RF;
+ }else if(IsLine(cSensorData,MODE_F)){
+ juModeEna(mode);
+ mode = MODE_F;
+ }else if(IsLine(cSensorData,MODE_RB)){
+ juModeEna(mode);
+ mode = MODE_RB;
+ }else if(IsLine(cSensorData,MODE_B)){
+ juModeEna(mode);
+ mode = MODE_B;
+ }else if(IsLine(cSensorData,MODE_L)){
+ juModeEna(mode);
+ mode = MODE_L;
+ ucGoalCheckFlag=FLAG_ON;
+ }
+ }else{
+ if(IsLine(cSensorData,MODE_R)){
+ juModeEna(mode);
+ mode = MODE_R;
+ }else if(IsLine(cSensorData,MODE_RB)){
+ juModeEna(mode);
+ mode = MODE_RB;
+ }else if(IsLine(cSensorData,MODE_B)){
+ juModeEna(mode);
+ mode = MODE_B;
+ }else if(IsLine(cSensorData,MODE_RF)){
+ juModeEna(mode);
+ mode = MODE_RF;
+ }else if(IsLine(cSensorData,MODE_F)){
+ juModeEna(mode);
+ mode = MODE_F;
+ }else if(IsLine(cSensorData,MODE_L)){
+ juModeEna(mode);
+ mode = MODE_L;
+ ucGoalCheckFlag=FLAG_ON;
+ }
+ }
+ }else if(mode == MODE_RB){
+ if(change_before_mode==MODE_RB ||change_before_mode==MODE_R || change_before_mode==MODE_RF){
+// if(change_before_mode==MODE_B ||change_before_mode==MODE_LB){
+ if(IsLine(cSensorData,MODE_RB)){
+ juModeEna(mode);
+ mode = MODE_RB;
+ }else if(IsLine(cSensorData,MODE_R)){
+ juModeEna(mode);
+ mode = MODE_R;
+ }else if(IsLine(cSensorData,MODE_RF)){
+ juModeEna(mode);
+ mode = MODE_RF;
+ }else if(IsLine(cSensorData,MODE_B)){
+ juModeEna(mode);
+ mode = MODE_B;
+ }else if(IsLine(cSensorData,MODE_RB)){
+ juModeEna(mode);
+ mode = MODE_RB;
+ }else if(IsLine(cSensorData,MODE_LF)){
+ juModeEna(mode);
+ mode = MODE_LF;
+ ucGoalCheckFlag=FLAG_ON;
+ }
+ }else{
+ if(IsLine(cSensorData,MODE_RB)){
+ juModeEna(mode);
+ mode = MODE_RB;
+ }else if(IsLine(cSensorData,MODE_B)){
+ juModeEna(mode);
+ mode = MODE_B;
+ }else if(IsLine(cSensorData,MODE_LB)){
+ juModeEna(mode);
+ mode = MODE_LB;
+ }else if(IsLine(cSensorData,MODE_R)){
+ juModeEna(mode);
+ mode = MODE_R;
+ }else if(IsLine(cSensorData,MODE_RF)){
+ juModeEna(mode);
+ mode = MODE_RF;
+ }else if(IsLine(cSensorData,MODE_LF)){
+ juModeEna(mode);
+ mode = MODE_LF;
+ ucGoalCheckFlag=FLAG_ON;
+ }
+ }
+ }else if(mode == MODE_B){
+// if(change_before_mode==MODE_B ||change_before_mode==MODE_RB || change_before_mode==MODE_R){
+ if(change_before_mode==MODE_LB ||change_before_mode==MODE_L){
+ if(IsLine(cSensorData,MODE_B)){
+ juModeEna(mode);
+ mode = MODE_B;
+ }else if(IsLine(cSensorData,MODE_RB)){
+ juModeEna(mode);
+ mode = MODE_RB;
+ }else if(IsLine(cSensorData,MODE_R)){
+ juModeEna(mode);
+ mode = MODE_R;
+ }else if(IsLine(cSensorData,MODE_LB)){
+ juModeEna(mode);
+ mode = MODE_LB;
+ }else if(IsLine(cSensorData,MODE_L)){
+ juModeEna(mode);
+ mode = MODE_L;
+ }else if(IsLine(cSensorData,MODE_F)){
+ juModeEna(mode);
+ mode = MODE_F;
+ ucGoalCheckFlag=FLAG_ON;
+ }
+ }else{
+ if(IsLine(cSensorData,MODE_B)){
+ juModeEna(mode);
+ mode = MODE_B;
+ }else if(IsLine(cSensorData,MODE_LB)){
+ juModeEna(mode);
+ mode = MODE_LB;
+ }else if(IsLine(cSensorData,MODE_L)){
+ juModeEna(mode);
+ mode = MODE_L;
+ }else if(IsLine(cSensorData,MODE_RB)){
+ juModeEna(mode);
+ mode = MODE_RB;
+ }else if(IsLine(cSensorData,MODE_R)){
+ juModeEna(mode);
+ mode = MODE_R;
+ }else if(IsLine(cSensorData,MODE_F)){
+ juModeEna(mode);
+ mode = MODE_F;
+ ucGoalCheckFlag=FLAG_ON;
+ }
+ }
+ }else if(mode == MODE_LB){
+ if(change_before_mode==MODE_LB ||change_before_mode==MODE_B || change_before_mode==MODE_RB){
+// if(change_before_mode==MODE_L ||change_before_mode==MODE_LF){
+ if(IsLine(cSensorData,MODE_LB)){
+ juModeEna(mode);
+ mode = MODE_LB;
+ }else if(IsLine(cSensorData,MODE_B)){
+ juModeEna(mode);
+ mode = MODE_B;
+ }else if(IsLine(cSensorData,MODE_RB)){
+ juModeEna(mode);
+ mode = MODE_RB;
+ }else if(IsLine(cSensorData,MODE_L)){
+ juModeEna(mode);
+ mode = MODE_L;
+ }else if(IsLine(cSensorData,MODE_LF)){
+ juModeEna(mode);
+ mode = MODE_LF;
+ }else if(IsLine(cSensorData,MODE_RF)){
+ juModeEna(mode);
+ mode = MODE_LF;
+ ucGoalCheckFlag=FLAG_ON;
+ }
+ }else{
+ if(IsLine(cSensorData,MODE_LB)){
+ juModeEna(mode);
+ mode = MODE_LB;
+ }else if(IsLine(cSensorData,MODE_L)){
+ juModeEna(mode);
+ mode = MODE_L;
+ }else if(IsLine(cSensorData,MODE_LF)){
+ juModeEna(mode);
+ mode = MODE_LF;
+ }else if(IsLine(cSensorData,MODE_B)){
+ juModeEna(mode);
+ mode = MODE_B;
+ }else if(IsLine(cSensorData,MODE_RB)){
+ juModeEna(mode);
+ mode = MODE_RB;
+ }else if(IsLine(cSensorData,MODE_RF)){
+ juModeEna(mode);
+ mode = MODE_LB;
+ ucGoalCheckFlag=FLAG_ON;
+ }
+ }
+ }else if(mode == MODE_L){
+// if(change_before_mode==MODE_L ||change_before_mode==MODE_LB || change_before_mode==MODE_B){
+ if(change_before_mode==MODE_LF ||change_before_mode==MODE_F){
+ if(IsLine(cSensorData,MODE_L)){
+ juModeEna(mode);
+ mode = MODE_L;
+ }else if(IsLine(cSensorData,MODE_LF)){
+ juModeEna(mode);
+ mode = MODE_LF;
+ }else if(IsLine(cSensorData,MODE_F)){
+ juModeEna(mode);
+ mode = MODE_F;
+ }else if(IsLine(cSensorData,MODE_LB)){
+ juModeEna(mode);
+ mode = MODE_LB;
+ }else if(IsLine(cSensorData,MODE_B)){
+ juModeEna(mode);
+ mode = MODE_B;
+ }else if(IsLine(cSensorData,MODE_R)){
+ juModeEna(mode);
+ mode = MODE_R;
+ ucGoalCheckFlag=FLAG_ON;
+ }
+ }else{
+ if(IsLine(cSensorData,MODE_L)){
+ juModeEna(mode);
+ mode = MODE_L;
+ }else if(IsLine(cSensorData,MODE_LF)){
+ juModeEna(mode);
+ mode = MODE_LF;
+ }else if(IsLine(cSensorData,MODE_F)){
+ juModeEna(mode);
+ mode = MODE_F;
+ }else if(IsLine(cSensorData,MODE_LB)){
+ juModeEna(mode);
+ mode = MODE_LB;
+ }else if(IsLine(cSensorData,MODE_B)){
+ juModeEna(mode);
+ mode = MODE_B;
+ }else if(IsLine(cSensorData,MODE_R)){
+ juModeEna(mode);
+ mode = MODE_R;
+ ucGoalCheckFlag=FLAG_ON;
+ }
+ }
+ }else if(mode == MODE_LF)
+ {
+ if(change_before_mode==MODE_LF ||change_before_mode==MODE_L || change_before_mode==MODE_LB){
+// if(change_before_mode==MODE_F ||change_before_mode==MODE_RF){
+ if(IsLine(cSensorData,MODE_LF)){
+ juModeEna(mode);
+ mode = MODE_LF;
+ }else if(IsLine(cSensorData,MODE_L)){
+ juModeEna(mode);
+ mode = MODE_L;
+ }else if(IsLine(cSensorData,MODE_LB)){
+ juModeEna(mode);
+ mode = MODE_LB;
+ }else if(IsLine(cSensorData,MODE_F)){
+ juModeEna(mode);
+ mode = MODE_F;
+ }else if(IsLine(cSensorData,MODE_RF)){
+ juModeEna(mode);
+ mode = MODE_RF;
+ }else if(IsLine(cSensorData,MODE_RB)){
+ juModeEna(mode);
+ mode = MODE_LF;
+ ucGoalCheckFlag=FLAG_ON;
+ }else if(IsLine(cSensorData,MODE_B)){
+ juModeEna(mode);
+ mode = MODE_LF;
+ ucGoalCheckFlag=FLAG_ON;
+ }else if(IsLine(cSensorData,MODE_R)){
+ juModeEna(mode);
+ mode = MODE_LF;
+ ucGoalCheckFlag=FLAG_ON;
+ }
+ }else{
+ if(IsLine(cSensorData,MODE_LF)){
+ juModeEna(mode);
+ mode = MODE_LF;
+ }else if(IsLine(cSensorData,MODE_F)){
+ juModeEna(mode);
+ mode = MODE_F;
+ }else if(IsLine(cSensorData,MODE_RF)){
+ juModeEna(mode);
+ mode = MODE_RF;
+ }else if(IsLine(cSensorData,MODE_L)){
+ juModeEna(mode);
+ mode = MODE_L;
+ }else if(IsLine(cSensorData,MODE_LB)){
+ juModeEna(mode);
+ mode = MODE_LB;
+ }else if(IsLine(cSensorData,MODE_RB)){
+ juModeEna(mode);
+ mode = MODE_LF;
+ ucGoalCheckFlag=FLAG_ON;
+ }else if(IsLine(cSensorData,MODE_R)){
+ juModeEna(mode);
+ mode = MODE_LF;
+ ucGoalCheckFlag=FLAG_ON;
+ }else if(IsLine(cSensorData,MODE_B)){
+ juModeEna(mode);
+ mode = MODE_LF;
+ ucGoalCheckFlag=FLAG_ON;
+ }
+ }
+ }
+
+ if(cSensorData == 0 && ucBSensorData != 0)
+ {
+ switch(mode){
+ case MODE_F:
+ mode = MODE_B;
+ juModeEna(mode);
+ break;
+ case MODE_RF:
+ mode = MODE_LB;
+ juModeEna(mode);
+ break;
+ case MODE_R:
+ mode = MODE_L;
+ juModeEna(mode);
+ break;
+ case MODE_RB:
+ mode = MODE_LF;
+ juModeEna(mode);
+ break;
+ case MODE_B:
+ mode = MODE_F;
+ juModeEna(mode);
+ break;
+ case MODE_LB:
+ mode = MODE_RF;
+ juModeEna(mode);
+ break;
+ case MODE_L:
+ mode = MODE_R;
+ juModeEna(mode);
+ break;
+ case MODE_LF:
+ mode = MODE_RB;
+ juModeEna(mode);
+ break;
+ }
+ //前後左右のみ前回値を保持する
+ if(MODE_F==mode || MODE_L==mode || MODE_R==mode || MODE_B==mode){
+ change_before_mode = mode;
+ }
+ }
+ ucBSensorData = cSensorData;
+
+ //ゴール判定(誤判定防止のため2回以上同じ入力の場合にモードを推移する)
+ if(ucGoalCheckFlag == FLAG_ON){
+ ucGoalCnt++;
+ if(ucGoalCnt >= GOAL_DELAY){
+ runMode = BONUS_MODE;
+ }
+ }else{
+ ucGoalCnt=0;
+ }
+ //mode 現在の進行方向
+ // brefore_mode 前回の進行方向
+
+ if(before_mode != mode)
+ {
+ change_before_mode = before_mode;
+ }
+
+ switch(mode){
+ case MODE_F:
+ moveF();
+ break;
+ case MODE_RF:
+ moveFR();
+ break;
+ case MODE_LF:
+ moveFL();
+ break;
+ case MODE_R:
+ moveR();
+ break;
+ case MODE_L:
+ moveL();
+ break;
+ case MODE_RB:
+ moveBR();
+ break;
+ case MODE_LB:
+ moveBL();
+ break;
+ case MODE_B:
+ moveB();
+ break;
+ default:
+ moveStop();
+ break;
+ }
+ return mode;
+}
+*/
+
+//センサーの動作ロジック
+//２段階の前回値保持ロジック
+int setMove(char cSensorData)
+{
+ static unsigned char ucGoalCnt = 0;
+ static unsigned char before_mode = 0;
+ static unsigned char change_before_mode = MODE_F;
+ static unsigned char ucBSensorData = 0xFF;
+ unsigned char ucGoalCheckFlag = FLAG_OFF;
+
+ //センサ入力に応じて進行方向を制御する
+ before_mode = mode;
+
+ if(mode == MODE_F){
+ if(change_before_mode==MODE_L){
+ if(IsLine(cSensorData,MODE_F)){
+ juModeEna(mode);
+ mode = MODE_F;
+ }else if(IsLine(cSensorData,MODE_RF)){
+ juModeEna(mode);
+ mode = MODE_RF;
+ }else if(IsLine(cSensorData,MODE_LF)){
+ juModeEna(mode);
+ mode = MODE_LF;
+ }else if(IsLine(cSensorData,MODE_R)){
+ juModeEna(mode);
+ mode = MODE_R;
+ }else if(IsLine(cSensorData,MODE_L)){
+ juModeEna(mode);
+ mode = MODE_L;
+ }else if(IsLine(cSensorData,MODE_B)){
+ juModeEna(mode);
+ mode = MODE_F;
+ ucGoalCheckFlag=FLAG_ON;
+ }else if(IsLine(cSensorData,MODE_RB)){
+ juModeEna(mode);
+ mode = MODE_F;
+ ucGoalCheckFlag=FLAG_ON;
+ }else if(IsLine(cSensorData,MODE_LB)){
+ juModeEna(mode);
+ mode = MODE_F;
+ ucGoalCheckFlag=FLAG_ON;
+ }
+ }else{
+ if(IsLine(cSensorData,MODE_F)){
+ juModeEna(mode);
+ mode = MODE_F;
+ }else if(IsLine(cSensorData,MODE_LF)){
+ juModeEna(mode);
+ mode = MODE_LF;
+ }else if(IsLine(cSensorData,MODE_RF)){
+ juModeEna(mode);
+ mode = MODE_RF;
+ }else if(IsLine(cSensorData,MODE_L)){
+ juModeEna(mode);
+ mode = MODE_L;
+ }else if(IsLine(cSensorData,MODE_R)){
+ juModeEna(mode);
+ mode = MODE_R;
+ }else if(IsLine(cSensorData,MODE_B)){
+ juModeEna(mode);
+ mode = MODE_F;
+ ucGoalCheckFlag=FLAG_ON;
+ }else if(IsLine(cSensorData,MODE_LB)){
+ juModeEna(mode);
+ mode = MODE_F;
+ ucGoalCheckFlag=FLAG_ON;
+ }else if(IsLine(cSensorData,MODE_RB)){
+ juModeEna(mode);
+ mode = MODE_F;
+ ucGoalCheckFlag=FLAG_ON;
+ }
+ }
+ }else if(mode == MODE_RF){
+ if(change_before_mode==MODE_R){
+ if(IsLine(cSensorData,MODE_RF)){
+ juModeEna(mode);
+ mode = MODE_RF;
+ }else if(IsLine(cSensorData,MODE_R)){
+ juModeEna(mode);
+ mode = MODE_R;
+ }else if(IsLine(cSensorData,MODE_F)){
+ juModeEna(mode);
+ mode = MODE_F;
+ }else if(IsLine(cSensorData,MODE_RB)){
+ juModeEna(mode);
+ mode = MODE_RB;
+ }else if(IsLine(cSensorData,MODE_LF)){
+ juModeEna(mode);
+ mode = MODE_LF;
+ }else if(IsLine(cSensorData,MODE_B)){
+ juModeEna(mode);
+ mode = MODE_RF;
+ ucGoalCheckFlag=FLAG_ON;
+ }else if(IsLine(cSensorData,MODE_L)){
+ juModeEna(mode);
+ mode = MODE_RF;
+ ucGoalCheckFlag=FLAG_ON;
+ }else if(IsLine(cSensorData,MODE_LB)){
+ juModeEna(mode);
+ mode = MODE_RF;
+ ucGoalCheckFlag=FLAG_ON;
+ }
+ }else{
+ if(IsLine(cSensorData,MODE_RF)){
+ juModeEna(mode);
+ mode = MODE_RF;
+ }else if(IsLine(cSensorData,MODE_F)){
+ juModeEna(mode);
+ mode = MODE_F;
+ }else if(IsLine(cSensorData,MODE_R)){
+ juModeEna(mode);
+ mode = MODE_R;
+ }else if(IsLine(cSensorData,MODE_LF)){
+ juModeEna(mode);
+ mode = MODE_LF;
+ }else if(IsLine(cSensorData,MODE_RB)){
+ juModeEna(mode);
+ mode = MODE_RB;
+ }else if(IsLine(cSensorData,MODE_L)){
+ juModeEna(mode);
+ mode = MODE_RF;
+ ucGoalCheckFlag=FLAG_ON;
+ }else if(IsLine(cSensorData,MODE_B)){
+ juModeEna(mode);
+ mode = MODE_RF;
+ ucGoalCheckFlag=FLAG_ON;
+ }else if(IsLine(cSensorData,MODE_LB)){
+ juModeEna(mode);
+ mode = MODE_RF;
+ ucGoalCheckFlag=FLAG_ON;
+ }
+ }
+ }else if(mode == MODE_R){
+ if(change_before_mode==MODE_R || change_before_mode==MODE_F){
+ if(IsLine(cSensorData,MODE_R)){
+ juModeEna(mode);
+ mode = MODE_R;
+ }else if(IsLine(cSensorData,MODE_RF)){
+ juModeEna(mode);
+ mode = MODE_RF;
+ }else if(IsLine(cSensorData,MODE_RB)){
+ juModeEna(mode);
+ mode = MODE_RB;
+ }else if(IsLine(cSensorData,MODE_F)){
+ juModeEna(mode);
+ mode = MODE_F;
+ }else if(IsLine(cSensorData,MODE_B)){
+ juModeEna(mode);
+ mode = MODE_B;
+ }else if(IsLine(cSensorData,MODE_L)){
+ juModeEna(mode);
+ mode = MODE_L;
+ ucGoalCheckFlag=FLAG_ON;
+ }
+ }else{
+ if(IsLine(cSensorData,MODE_R)){
+ juModeEna(mode);
+ mode = MODE_R;
+ }else if(IsLine(cSensorData,MODE_RB)){
+ juModeEna(mode);
+ mode = MODE_RB;
+ }else if(IsLine(cSensorData,MODE_RF)){
+ juModeEna(mode);
+ mode = MODE_RF;
+ }else if(IsLine(cSensorData,MODE_B)){
+ juModeEna(mode);
+ mode = MODE_B;
+ }else if(IsLine(cSensorData,MODE_F)){
+ juModeEna(mode);
+ mode = MODE_F;
+ }else if(IsLine(cSensorData,MODE_L)){
+ juModeEna(mode);
+ mode = MODE_L;
+ ucGoalCheckFlag=FLAG_ON;
+ }
+ }
+ }else if(mode == MODE_RB){
+ if(change_before_mode==MODE_R){
+ if(IsLine(cSensorData,MODE_RB)){
+ juModeEna(mode);
+ mode = MODE_RB;
+ }else if(IsLine(cSensorData,MODE_R)){
+ juModeEna(mode);
+ mode = MODE_R;
+ }else if(IsLine(cSensorData,MODE_B)){
+ juModeEna(mode);
+ mode = MODE_B;
+ }else if(IsLine(cSensorData,MODE_RF)){
+ juModeEna(mode);
+ mode = MODE_RF;
+ }else if(IsLine(cSensorData,MODE_LB)){
+ juModeEna(mode);
+ mode = MODE_LB;
+ }else if(IsLine(cSensorData,MODE_LF)){
+ juModeEna(mode);
+ mode = MODE_LF;
+ ucGoalCheckFlag=FLAG_ON;
+ }
+ }else{
+ if(IsLine(cSensorData,MODE_RB)){
+ juModeEna(mode);
+ mode = MODE_RB;
+ }else if(IsLine(cSensorData,MODE_B)){
+ juModeEna(mode);
+ mode = MODE_B;
+ }else if(IsLine(cSensorData,MODE_R)){
+ juModeEna(mode);
+ mode = MODE_R;
+ }else if(IsLine(cSensorData,MODE_LB)){
+ juModeEna(mode);
+ mode = MODE_LB;
+ }else if(IsLine(cSensorData,MODE_RF)){
+ juModeEna(mode);
+ mode = MODE_RF;
+ }else if(IsLine(cSensorData,MODE_LF)){
+ juModeEna(mode);
+ mode = MODE_LF;
+ ucGoalCheckFlag=FLAG_ON;
+ }
+ }
+ }else if(mode == MODE_B){
+ if(change_before_mode==MODE_L){
+ if(IsLine(cSensorData,MODE_B)){
+ juModeEna(mode);
+ mode = MODE_B;
+ }else if(IsLine(cSensorData,MODE_RB)){
+ juModeEna(mode);
+ mode = MODE_RB;
+ }else if(IsLine(cSensorData,MODE_LB)){
+ juModeEna(mode);
+ mode = MODE_LB;
+ }else if(IsLine(cSensorData,MODE_R)){
+ juModeEna(mode);
+ mode = MODE_R;
+ }else if(IsLine(cSensorData,MODE_L)){
+ juModeEna(mode);
+ mode = MODE_L;
+ }else if(IsLine(cSensorData,MODE_F)){
+ juModeEna(mode);
+ mode = MODE_F;
+ ucGoalCheckFlag=FLAG_ON;
+ }
+ }else{
+ if(IsLine(cSensorData,MODE_B)){
+ juModeEna(mode);
+ mode = MODE_B;
+ }else if(IsLine(cSensorData,MODE_LB)){
+ juModeEna(mode);
+ mode = MODE_LB;
+ }else if(IsLine(cSensorData,MODE_RB)){
+ juModeEna(mode);
+ mode = MODE_RB;
+ }else if(IsLine(cSensorData,MODE_L)){
+ juModeEna(mode);
+ mode = MODE_L;
+ }else if(IsLine(cSensorData,MODE_R)){
+ juModeEna(mode);
+ mode = MODE_R;
+ }else if(IsLine(cSensorData,MODE_F)){
+ juModeEna(mode);
+ mode = MODE_F;
+ ucGoalCheckFlag=FLAG_ON;
+ }
+ }
+ }else if(mode == MODE_LB){
+ if(change_before_mode==MODE_B){
+ if(IsLine(cSensorData,MODE_LB)){
+ juModeEna(mode);
+ mode = MODE_LB;
+ }else if(IsLine(cSensorData,MODE_B)){
+ juModeEna(mode);
+ mode = MODE_B;
+ }else if(IsLine(cSensorData,MODE_L)){
+ juModeEna(mode);
+ mode = MODE_L;
+ }else if(IsLine(cSensorData,MODE_RB)){
+ juModeEna(mode);
+ mode = MODE_RB;
+ }else if(IsLine(cSensorData,MODE_LF)){
+ juModeEna(mode);
+ mode = MODE_LF;
+ }else if(IsLine(cSensorData,MODE_RF)){
+ juModeEna(mode);
+ mode = MODE_LF;
+ ucGoalCheckFlag=FLAG_ON;
+ }
+ }else{
+ if(IsLine(cSensorData,MODE_LB)){
+ juModeEna(mode);
+ mode = MODE_LB;
+ }else if(IsLine(cSensorData,MODE_L)){
+ juModeEna(mode);
+ mode = MODE_L;
+ }else if(IsLine(cSensorData,MODE_B)){
+ juModeEna(mode);
+ mode = MODE_B;
+ }else if(IsLine(cSensorData,MODE_LF)){
+ juModeEna(mode);
+ mode = MODE_LF;
+ }else if(IsLine(cSensorData,MODE_RB)){
+ juModeEna(mode);
+ mode = MODE_RB;
+ }else if(IsLine(cSensorData,MODE_RF)){
+ juModeEna(mode);
+ mode = MODE_LB;
+ ucGoalCheckFlag=FLAG_ON;
+ }
+ }
+ }else if(mode == MODE_L){
+ if(change_before_mode==MODE_L || change_before_mode==MODE_B){
+ if(IsLine(cSensorData,MODE_L)){
+ juModeEna(mode);
+ mode = MODE_L;
+ }else if(IsLine(cSensorData,MODE_LB)){
+ juModeEna(mode);
+ mode = MODE_LB;
+ }else if(IsLine(cSensorData,MODE_LF)){
+ juModeEna(mode);
+ mode = MODE_LF;
+ }else if(IsLine(cSensorData,MODE_B)){
+ juModeEna(mode);
+ mode = MODE_B;
+ }else if(IsLine(cSensorData,MODE_F)){
+ juModeEna(mode);
+ mode = MODE_F;
+ }else if(IsLine(cSensorData,MODE_R)){
+ juModeEna(mode);
+ mode = MODE_R;
+ ucGoalCheckFlag=FLAG_ON;
+ }
+ }else{
+ if(IsLine(cSensorData,MODE_L)){
+ juModeEna(mode);
+ mode = MODE_L;
+ }else if(IsLine(cSensorData,MODE_LF)){
+ juModeEna(mode);
+ mode = MODE_LF;
+ }else if(IsLine(cSensorData,MODE_LB)){
+ juModeEna(mode);
+ mode = MODE_LB;
+ }else if(IsLine(cSensorData,MODE_F)){
+ juModeEna(mode);
+ mode = MODE_F;
+ }else if(IsLine(cSensorData,MODE_B)){
+ juModeEna(mode);
+ mode = MODE_B;
+ }else if(IsLine(cSensorData,MODE_R)){
+ juModeEna(mode);
+ mode = MODE_R;
+ ucGoalCheckFlag=FLAG_ON;
+ }
+ }
+ }else if(mode == MODE_LF)
+ {
+ if(change_before_mode==MODE_F){
+ if(IsLine(cSensorData,MODE_LF)){
+ juModeEna(mode);
+ mode = MODE_LF;
+ }else if(IsLine(cSensorData,MODE_F)){
+ juModeEna(mode);
+ mode = MODE_F;
+ }else if(IsLine(cSensorData,MODE_L)){
+ juModeEna(mode);
+ mode = MODE_L;
+ }else if(IsLine(cSensorData,MODE_RF)){
+ juModeEna(mode);
+ mode = MODE_RF;
+ }else if(IsLine(cSensorData,MODE_LB)){
+ juModeEna(mode);
+ mode = MODE_LB;
+ }else if(IsLine(cSensorData,MODE_RB)){
+ juModeEna(mode);
+ mode = MODE_LF;
+ ucGoalCheckFlag=FLAG_ON;
+ }else if(IsLine(cSensorData,MODE_B)){
+ juModeEna(mode);
+ mode = MODE_LF;
+ ucGoalCheckFlag=FLAG_ON;
+ }else if(IsLine(cSensorData,MODE_R)){
+ juModeEna(mode);
+ mode = MODE_LF;
+ ucGoalCheckFlag=FLAG_ON;
+ }
+ }else{
+ if(IsLine(cSensorData,MODE_LF)){
+ juModeEna(mode);
+ mode = MODE_LF;
+ }else if(IsLine(cSensorData,MODE_L)){
+ juModeEna(mode);
+ mode = MODE_L;
+ }else if(IsLine(cSensorData,MODE_F)){
+ juModeEna(mode);
+ mode = MODE_F;
+ }else if(IsLine(cSensorData,MODE_LB)){
+ juModeEna(mode);
+ mode = MODE_LB;
+ }else if(IsLine(cSensorData,MODE_RF)){
+ juModeEna(mode);
+ mode = MODE_RF;
+ }else if(IsLine(cSensorData,MODE_RB)){
+ juModeEna(mode);
+ mode = MODE_LF;
+ ucGoalCheckFlag=FLAG_ON;
+ }else if(IsLine(cSensorData,MODE_R)){
+ juModeEna(mode);
+ mode = MODE_LF;
+ ucGoalCheckFlag=FLAG_ON;
+ }else if(IsLine(cSensorData,MODE_B)){
+ juModeEna(mode);
+ mode = MODE_LF;
+ ucGoalCheckFlag=FLAG_ON;
+ }
+ }
+ }
+
+ if(cSensorData == 0 && ucBSensorData != 0)
+ {
+ switch(mode){
+ case MODE_F:
+ mode = MODE_B;
+ juModeEna(mode);
+ break;
+ case MODE_RF:
+ mode = MODE_LB;
+ juModeEna(mode);
+ break;
+ case MODE_R:
+ mode = MODE_L;
+ juModeEna(mode);
+ break;
+ case MODE_RB:
+ mode = MODE_LF;
+ juModeEna(mode);
+ break;
+ case MODE_B:
+ mode = MODE_F;
+ juModeEna(mode);
+ break;
+ case MODE_LB:
+ mode = MODE_RF;
+ juModeEna(mode);
+ break;
+ case MODE_L:
+ mode = MODE_R;
+ juModeEna(mode);
+ break;
+ case MODE_LF:
+ mode = MODE_RB;
+ juModeEna(mode);
+ break;
+ }
+ }
+ ucBSensorData = cSensorData;
+
+ //ゴール判定(誤判定防止のため2回以上同じ入力の場合にモードを推移する)
+ if(ucGoalCheckFlag == FLAG_ON){
+ ucGoalCnt++;
+ if(ucGoalCnt >= GOAL_DELAY){
+ runMode = BONUS_MODE;
+ }
+ }else{
+ ucGoalCnt=0;
+ }
+
+ //mode 現在の進行方向
+ // brefore_mode 前回の進行方向
+
+ if(before_mode != mode)
+ {
+ //前後左右のみ前回値を保持する
+ if(MODE_F==before_mode || MODE_L==before_mode || MODE_R==before_mode || MODE_B==before_mode){
+ change_before_mode = before_mode;
+ }
+ }
+
+ switch(mode){
+ case MODE_F:
+ moveF();
+ break;
+ case MODE_RF:
+ moveFR();
+ break;
+ case MODE_LF:
+ moveFL();
+ break;
+ case MODE_R:
+ moveR();
+ break;
+ case MODE_L:
+ moveL();
+ break;
+ case MODE_RB:
+ moveBR();
+ break;
+ case MODE_LB:
+ moveBL();
+ break;
+ case MODE_B:
+ moveB();
+ break;
+ default:
+ moveStop();
+ break;
+ }
+ return mode;
+}
+
+/*
+//センサーの動作ロジック
+//２段階の前回値保持ロジック
+int setMove(char cSensorData)
+{
+ if(mode == MODE_F){
+ if(IsLine(cSensorData,MODE_F)){
+ juModeEna(mode);
+ mode = MODE_F;
+ }else if(IsLine(cSensorData,MODE_RF)){
+ juModeEna(mode);
+ mode = MODE_RF;
+ }else if(IsLine(cSensorData,MODE_LF)){
+ juModeEna(mode);
+ mode = MODE_LF;
+ }else if(IsLine(cSensorData,MODE_R)){
+ juModeEna(mode);
+ mode = MODE_R;
+ }else if(IsLine(cSensorData,MODE_L)){
+ juModeEna(mode);
+ mode = MODE_L;
+ }else if(IsLine(cSensorData,MODE_B)){
+ juModeEna(mode);
+ mode = MODE_B;
+ runMode= BONUS_MODE;
+ }
+ }else if(mode == MODE_RF){
+ if(IsLine(cSensorData,MODE_RF)){
+ juModeEna(mode);
+ mode = MODE_RF;
+ }else if(IsLine(cSensorData,MODE_R)){
+ juModeEna(mode);
+ mode = MODE_R;
+ }else if(IsLine(cSensorData,MODE_F)){
+ juModeEna(mode);
+ mode = MODE_F;
+ }else if(IsLine(cSensorData,MODE_LB)){
+ juModeEna(mode);
+ mode = MODE_LB;
+ runMode= BONUS_MODE;
+ }
+ }else if(mode == MODE_R){
+ if(IsLine(cSensorData,MODE_R)){
+ juModeEna(mode);
+ mode = MODE_R;
+ }else if(IsLine(cSensorData,MODE_RF)){
+ juModeEna(mode);
+ mode = MODE_RF;
+ }else if(IsLine(cSensorData,MODE_RB)){
+ juModeEna(mode);
+ mode = MODE_RB;
+ }else if(IsLine(cSensorData,MODE_F)){
+ juModeEna(mode);
+ mode = MODE_F;
+ }else if(IsLine(cSensorData,MODE_B)){
+ juModeEna(mode);
+ mode = MODE_B;
+ }else if(IsLine(cSensorData,MODE_L)){
+ juModeEna(mode);
+ mode = MODE_L;
+ runMode= BONUS_MODE;
+ }
+ }else if(mode == MODE_RB){
+ if(IsLine(cSensorData,MODE_RB)){
+ juModeEna(mode);
+ mode = MODE_RB;
+ }else if(IsLine(cSensorData,MODE_R)){
+ juModeEna(mode);
+ mode = MODE_R;
+ }else if(IsLine(cSensorData,MODE_B)){
+ juModeEna(mode);
+ mode = MODE_B;
+ }else if(IsLine(cSensorData,MODE_LF)){
+ juModeEna(mode);
+ mode = MODE_LF;
+ runMode= BONUS_MODE;
+ }
+ }else if(mode == MODE_B){
+ if(IsLine(cSensorData,MODE_B)){
+ juModeEna(mode);
+ mode = MODE_B;
+ }else if(IsLine(cSensorData,MODE_RB)){
+ juModeEna(mode);
+ mode = MODE_RB;
+ }else if(IsLine(cSensorData,MODE_LB)){
+ juModeEna(mode);
+ mode = MODE_LB;
+ }else if(IsLine(cSensorData,MODE_R)){
+ juModeEna(mode);
+ mode = MODE_R;
+ }else if(IsLine(cSensorData,MODE_L)){
+ juModeEna(mode);
+ mode = MODE_L;
+ }else if(IsLine(cSensorData,MODE_F)){
+ juModeEna(mode);
+ mode = MODE_F;
+ runMode= BONUS_MODE;
+ }
+ }else if(mode == MODE_LB){
+ if(IsLine(cSensorData,MODE_LB)){
+ juModeEna(mode);
+ mode = MODE_LB;
+ }else if(IsLine(cSensorData,MODE_B)){
+ juModeEna(mode);
+ mode = MODE_B;
+ }else if(IsLine(cSensorData,MODE_L)){
+ juModeEna(mode);
+ mode = MODE_L;
+ }else if(IsLine(cSensorData,MODE_RF)){
+ juModeEna(mode);
+ mode = MODE_LF;
+ runMode= BONUS_MODE;
+ }
+ }else if(mode == MODE_L){
+ if(IsLine(cSensorData,MODE_L)){
+ juModeEna(mode);
+ mode = MODE_L;
+ }else if(IsLine(cSensorData,MODE_LB)){
+ juModeEna(mode);
+ mode = MODE_LB;
+ }else if(IsLine(cSensorData,MODE_LF)){
+ juModeEna(mode);
+ mode = MODE_LF;
+ }else if(IsLine(cSensorData,MODE_B)){
+ juModeEna(mode);
+ mode = MODE_B;
+ }else if(IsLine(cSensorData,MODE_F)){
+ juModeEna(mode);
+ mode = MODE_F;
+ }else if(IsLine(cSensorData,MODE_R)){
+ juModeEna(mode);
+ mode = MODE_R;
+ runMode= BONUS_MODE;
+ }
+ }else if(mode == MODE_LF)
+ {
+ if(IsLine(cSensorData,MODE_LF)){
+ juModeEna(mode);
+ mode = MODE_LF;
+ }else if(IsLine(cSensorData,MODE_L)){
+ juModeEna(mode);
+ mode = MODE_L;
+ }else if(IsLine(cSensorData,MODE_F)){
+ juModeEna(mode);
+ mode = MODE_F;
+ }else if(IsLine(cSensorData,MODE_RB)){
+ juModeEna(mode);
+ mode = MODE_RB;
+ runMode= BONUS_MODE;
+ }
+ }
+
+ switch(mode){
+ case MODE_F:
+ moveF();
+ break;
+ case MODE_RF:
+ moveFR();
+ break;
+ case MODE_LF:
+ moveFL();
+ break;
+ case MODE_R:
+ moveR();
+ break;
+ case MODE_L:
+ moveL();
+ break;
+ case MODE_RB:
+ moveBR();
+ break;
+ case MODE_LB:
+ moveBL();
+ break;
+ case MODE_B:
+ moveB();
+ break;
+ default:
+ moveStop();
+ break;
+ }
+ return mode;
+}
+
+*/
+void ReverseMove(int mode)
+{
+ switch(mode){
+ case MODE_F:
+ moveB();
+ break;
+ case MODE_RF:
+ moveBL();
+ break;
+ case MODE_LF:
+ moveBR();
+ break;
+ case MODE_R:
+ moveL();
+ break;
+ case MODE_L:
+ moveR();
+ break;
+ case MODE_RB:
+ moveFL();
+ break;
+ case MODE_LB:
+ moveFR();
+ break;
+ case MODE_B:
+ moveF();
+ break;
+ default:
+ moveStop();
+ break;
+ }
+}
+
+
+void moveStop()
+{
+ moterRF.pulsewidth_us(MOTOR_STOP_FR);
+ moterRB.pulsewidth_us(MOTOR_STOP_BR);
+ moterLB.pulsewidth_us(MOTOR_STOP_BL);
+ moterLF.pulsewidth_us(MOTOR_STOP_FL);
+}
+
+void moveF()
+{
+ moterRF.pulsewidth_us(MOTOR_FRONT_FR);
+ moterRB.pulsewidth_us(MOTOR_FRONT_BR);
+ moterLB.pulsewidth_us(MOTOR_FRONT_BL);
+ moterLF.pulsewidth_us(MOTOR_FRONT_FL);
+}
+void moveFR()
+{
+ moterRF.pulsewidth_us(MOTOR_FRONT_RIGHT_FR);
+ moterRB.pulsewidth_us(MOTOR_FRONT_RIGHT_BR);
+ moterLF.pulsewidth_us(MOTOR_FRONT_RIGHT_FL);
+ moterLB.pulsewidth_us(MOTOR_FRONT_RIGHT_BL);
+}
+void moveFL()
+{
+ moterRF.pulsewidth_us(MOTOR_FRONT_LEFT_FR);
+ moterRB.pulsewidth_us(MOTOR_FRONT_LEFT_BR);
+ moterLB.pulsewidth_us(MOTOR_FRONT_LEFT_BL);
+ moterLF.pulsewidth_us(MOTOR_FRONT_LEFT_FL);
+}
+
+void moveL()
+{
+ moterRF.pulsewidth_us(MOTOR_LEFT_FR);
+ moterRB.pulsewidth_us(MOTOR_LEFT_BR);
+ moterLB.pulsewidth_us(MOTOR_LEFT_BL);
+ moterLF.pulsewidth_us(MOTOR_LEFT_FL);
+}
+void moveR()
+{
+ moterRF.pulsewidth_us(MOTOR_RIGHT_FR);
+ moterRB.pulsewidth_us(MOTOR_RIGHT_BR);
+ moterLB.pulsewidth_us(MOTOR_RIGHT_BL);
+ moterLF.pulsewidth_us(MOTOR_RIGHT_FL);
+}
+void moveBR()
+{
+ moterRF.pulsewidth_us(MOTOR_BACK_RIGHT_FR);
+ moterRB.pulsewidth_us(MOTOR_BACK_RIGHT_BR);
+ moterLB.pulsewidth_us(MOTOR_BACK_RIGHT_BL);
+ moterLF.pulsewidth_us(MOTOR_BACK_RIGHT_FL);
+}
+void moveBL()
+{
+ moterRF.pulsewidth_us(MOTOR_BACK_LEFT_FR);
+ moterRB.pulsewidth_us(MOTOR_BACK_LEFT_BR);
+ moterLB.pulsewidth_us(MOTOR_BACK_LEFT_BL);
+ moterLF.pulsewidth_us(MOTOR_BACK_LEFT_FL);
+}
+void moveB()
+{
+ moterRF.pulsewidth_us(MOTOR_BACK_FR);
+ moterRB.pulsewidth_us(MOTOR_BACK_BR);
+ moterLB.pulsewidth_us(MOTOR_BACK_BL);
+ moterLF.pulsewidth_us(MOTOR_BACK_FL);
+}