Akira Minomo
Line Trace Robot Program Sagami
motor.cpp
- Committer:
- firlight1034
- Date:
- 2016-10-09
- Revision:
- 0:95bb05165e79
File content as of revision 0:95bb05165e79:
/******************************************************************************
@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);
}