大季 矢花
aa
System/Process/Process.cpp
- Committer:
- kishibekairohan
- Date:
- 2019-11-10
- Revision:
- 44:2aff0da5ebdb
- Parent:
- 43:1bb4fbe7d4c6
- Child:
- 45:0cf48a0605cd
File content as of revision 44:2aff0da5ebdb:
#include "mbed.h"
#include "Process.h"
#include <stdlib.h>
#include "../../CommonLibraries/PID/PID.h"
#include "../../Communication/RS485/ActuatorHub/ActuatorHub.h"
#include "../../Communication/RS485/LineHub/LineHub.h"
#include "../../Communication/Controller/Controller.h"
#include "../../Switch/Switch.h"
#include "../../LED/LED.h"
#include "../../Safty/Safty.h"
#include "../Using.h"
using namespace SWITCH;
using namespace PID_SPACE;
using namespace LINEHUB;
static CONTROLLER::ControllerData *controller;
ACTUATORHUB::MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];
ACTUATORHUB::SOLENOID::SolenoidStatus solenoid;
static bool lock;
static bool processChangeComp;
static int current;
static void AllActuatorReset();
#ifdef USE_SUBPROCESS
static void (*Process[USE_PROCESS_NUM])(void);
#endif
#pragma region USER-DEFINED_VARIABLES_AND_PROTOTYPE
/*Replace here with the definition code of your variables.*/
//Serial pc(USBTX, USBRX);
const int omni[15][15] = {
{ 0, 5, 21, 47, 83, 130, 187, 255, 255, 255, 255, 255, 255, 255, 255 },
{ -5, 0, 5, 21, 47, 83, 130, 187, 193, 208, 234, 255, 255, 255, 255 },
{ -21, -5, 0, 5, 21, 47, 83, 130, 135, 151, 177, 213, 255, 255, 255 },
{ -47, -21, 5, 0, 5, 21, 47, 83, 88, 104, 130, 167, 213, 255, 255 },
{ -83, -47, -21, 5, 0, 5, 21, 47, 52, 68, 94, 130, 177, 234, 255 },
{ -130, -83, -47, -21, 5, 0, 5, 21, 26, 42, 68, 104, 151, 208, 255 },
{ -187, -130, -83, -47, -21, -5, 0, 5, 10, 26, 52, 88, 135, 193, 255 },
{ -255, -187, -130, -83, -47, -21, -5, 0, 5, 21, 47, 83, 130, 187, 255 },
{ -255, -193, -135, -88, -52, -26, -10, -5, 0, 5, 21, 47, 83, 130, 187 },
{ -255, -208, -151, -104, -68, -42, -26, -21, -5, 0, 5, 21, 47, 83, 130 },
{ -255, -234, -177, -130, -94, -68, -52, -47, -21, -7, 0, 7, 21, 47, 83 },
{ -255, -255, -213, -167, -130, -104, -88, -83, -47, -21, -5, 0, 5, 21, 47 },
{ -255, -255, -255, -213, -177, -151, -135, -130, -83, -47, -21, -5, 0, 5, 21 },
{ -255, -255, -255, -255, -234, -208, -193, -187, -130, -83, -47, -21, -5, 0, 5 },
{ -255, -255, -255, -255, -255, -255, -255, -255, -187, -130, -83, -47, -21, -5, 0 }
};
const int curve[15] = { -204, -150, -104, -66, -38, -17, -4, 0, 4, 17, 38, 66, 104, 150, 204 };
uint8_t SetStatus(int pwmVal);
uint8_t SetPWM(int pwmVal);
DigitalIn EMC(PC_13); //非常停止確認用
bool start_flag = true;
bool UP_flag = false;
bool SW_flag = false;
bool Air_flag = false;
bool zyouge;
bool zyougedo;
bool dz2=true;
bool dz2i=false;
bool dz3=true;
bool dz3i=false;
bool dz4=true;
bool dz4i=false;
bool zone;
bool through=false;
bool counts=false;
bool mtc=false;
bool mtc2 = false;
bool emcs=false;
bool kiri;
int g[8];
int Twsh;
int testmode =0;
int mode =0;
int cross=0;//十字数える用
int cros=0;
int countss=100000;//wait代替え
///*********PWM調整用*********///
int fast = 60;
int normal = 40;
int slow = 20;
#pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE
#ifdef USE_SUBPROCESS
#if USE_PROCESS_NUM>0
static void Process0(void);
#endif
#if USE_PROCESS_NUM>1
static void Process1(void);
#endif
#if USE_PROCESS_NUM>2
static void Process2(void);
#endif
#if USE_PROCESS_NUM>3
static void Process3(void);
#endif
#if USE_PROCESS_NUM>4
static void Process4(void);
#endif
#if USE_PROCESS_NUM>5
static void Process5(void);
#endif
#if USE_PROCESS_NUM>6
static void Process6(void);
#endif
#if USE_PROCESS_NUM>7
static void Process7(void);
#endif
#if USE_PROCESS_NUM>8
static void Process8(void);
#endif
#if USE_PROCESS_NUM>9
static void Process9(void);
#endif
#endif
void SystemProcessInitialize()
{
#pragma region USER-DEFINED_VARIABLE_INIT
/*Replace here with the initialization code of your variables.*/
#pragma endregion USER-DEFINED_VARIABLE_INIT
lock = true;
processChangeComp = true;
current = DEFAULT_PROCESS;
#ifdef USE_SUBPROCESS
#if USE_PROCESS_NUM>0
Process[0] = Process0;
#endif
#if USE_PROCESS_NUM>1
Process[1] = Process1;
#endif
#if USE_PROCESS_NUM>2
Process[2] = Process2;
#endif
#if USE_PROCESS_NUM>3
Process[3] = Process3;
#endif
#if USE_PROCESS_NUM>4
Process[4] = Process4;
#endif
#if USE_PROCESS_NUM>5
Process[5] = Process5;
#endif
#if USE_PROCESS_NUM>6
Process[6] = Process6;
#endif
#if USE_PROCESS_NUM>7
Process[7] = Process7;
#endif
#if USE_PROCESS_NUM>8
Process[8] = Process8;
#endif
#if USE_PROCESS_NUM>9
Process[9] = Process9;
#endif
#endif
}
static void SystemProcessUpdate()
{
#ifdef USE_SUBPROCESS
if(controller->Button.HOME) lock = false;
if(controller->Button.START && processChangeComp) {
current++;
if (USE_PROCESS_NUM < current) current = USE_PROCESS_NUM;
processChangeComp = false;
} else if(controller->Button.SELECT && processChangeComp) {
current--;
if (current < 0) current = 0;
processChangeComp = false;
} else if(!controller->Button.SELECT && !controller->Button.START) processChangeComp = true;
#endif
#ifdef USE_MOTOR
ACTUATORHUB::MOTOR::Motor::Update(motor);
#endif
#ifdef USE_SOLENOID
ACTUATORHUB::SOLENOID::Solenoid::Update(solenoid);
#endif
#ifdef USE_RS485
ACTUATORHUB::ActuatorHub::Update();
//LINEHUB::LineHub::Update();
#endif
}
int point1;
void SystemProcess()
{
SystemProcessInitialize();
while(1) {
//pc.printf("4:%d\n\r",g[4]);
if(EMC==0) {
fast=40;
normal=30;
slow=20;
cross=0;//十字数える用
cros=0;
countss=0;//wait代替え
UP_flag = false;
SW_flag = false;
Air_flag = false;
zyouge=false;
zyougedo=false;
dz2=true;
dz2i=false;
dz3=true;
dz3i=false;
dz4=true;
dz4i=false;
through=false;
counts=false;
mtc=false;
mtc2 = false;
current = 0;
SystemProcessInitialize();
if(emcs==false) {
AllActuatorReset();
emcs=true;
}
} else {
emcs=false;
}
if(mode==0) {
if(ZoneSW) {
LED::LED::TapeledMode(1);
} else {
LED::LED::TapeledMode(2);
}
}
for(int i = 0; i < 8; i++) {
g[i] = LineHub::GetPara(i);
}
if(ThSW) {
Twsh=1;
} else {
Twsh=2;
}
if(StertSW && start_flag == true) {
start_flag = false;
lock = false;
if(ZoneSW) {
countss=0;
mode=1;
current = 3;//10GATU30変更した
} else {
countss = 0;
mode=1;
current = 2;
}
}
if(DWSW||DOSW) {
if(DWSW) {
zyouge=true;
}
if(DOSW) {
if(DOLS) {
motor[MOTOR_0].pwm = 100;
motor[MOTOR_0].dir = BRAKE;
} else {
motor[MOTOR_0].pwm = 200;
motor[MOTOR_0].dir = BACK;
}
}
} else if(zyouge==false) {
motor[MOTOR_0].pwm = 100;
motor[MOTOR_0].dir = BRAKE;
}
if(zyouge) {
if(Twsh==1) {
motor[MOTOR_0].pwm = 255;
motor[MOTOR_0].dir = FOR;
if(UPLS) {
motor[MOTOR_0].pwm = 100;
motor[MOTOR_0].dir = BRAKE;
zyouge=false;
}
}
if(Twsh==2) {
motor[MOTOR_0].pwm = 245;
motor[MOTOR_0].dir = FOR;
if(UPLS2) {
motor[MOTOR_0].pwm = 100;
motor[MOTOR_0].dir = BRAKE;
zyouge=false;
}
}
}
if(AIRSW) {
if(SW_flag==false) {
if(Air_flag==false) {
solenoid.solenoid3 = SOLENOID_ON;
Air_flag=true;
SW_flag=true;
} else {
solenoid.solenoid3 = SOLENOID_OFF;
Air_flag=false;
SW_flag=true;
}
}
} else {
if(SW_flag==true) {
countss++;
if(countss>=2000) {
SW_flag=false;
countss=0;
}
}
}
#ifdef USE_MU
controller = CONTROLLER::Controller::GetData();
#endif
#ifdef USE_ERRORCHECK
if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost & start_flag) {
CONTROLLER::Controller::DataReset();
AllActuatorReset();
lock = true;
} else
#endif
{
#ifdef USE_SUBPROCESS
if(!lock) {
Process[current]();
} else
#endif
{
//ロック時の処理
}
}
SystemProcessUpdate();
}
}
#pragma region PROCESS
#ifdef USE_SUBPROCESS
#if USE_PROCESS_NUM>0
static void Process0()
{
//AllActuatorReset();
}
#endif
#if USE_PROCESS_NUM>1
static void Process1()
{
LED::LED::TapeledMode(6);
start_flag = true;
mtc=false;
mtc2 = false;
mode=1;
cross=0;
Twsh=1;
motor[TIRE_FR].dir = SetStatus(-omni[controller->AnalogL.Y][14-controller->AnalogL.X] + curve[controller->AnalogR.X]);
motor[TIRE_FL].dir = SetStatus(omni[controller->AnalogL.Y][controller->AnalogL.X] + curve[controller->AnalogR.X]);
motor[TIRE_BR].dir = SetStatus(-omni[14-controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]);
motor[TIRE_BL].dir = SetStatus(omni[controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]);
motor[TIRE_FR].pwm = SetPWM(omni[controller->AnalogL.Y][14-controller->AnalogL.X]+ curve[controller->AnalogR.X])*0.2;
motor[TIRE_FL].pwm = SetPWM(omni[controller->AnalogL.Y][controller->AnalogL.X]+ curve[controller->AnalogR.X])*0.2;
motor[TIRE_BR].pwm = SetPWM(omni[14-controller->AnalogL.X][14-controller->AnalogL.Y]+ curve[controller->AnalogR.X])*0.2;
motor[TIRE_BL].pwm = SetPWM(omni[controller->AnalogL.X][14-controller->AnalogL.Y]+ curve[controller->AnalogR.X])*0.2;
if(controller->Button.UP||controller->Button.DOWN) {
if(controller->Button.UP) {
motor[MOTOR_0].pwm =200;
motor[MOTOR_0].dir = FOR;
if(UPLS) {
motor[MOTOR_0].pwm = 0;
motor[MOTOR_0].dir = BRAKE;
}
}
if(controller->Button.DOWN) {
motor[MOTOR_0].pwm = 50;
motor[MOTOR_0].dir = BACK;
}
} else {
motor[MOTOR_0].pwm = 0;
motor[MOTOR_0].dir = BRAKE;
}
if(controller->Button.Y) {
if(dz2==true) {
if(dz2i==false) {
solenoid.solenoid2 = SOLENOID_ON;
dz2i=true;
} else {
solenoid.solenoid2 = SOLENOID_OFF;
dz2i=false;
}
dz2=false;
}
} else {
dz2=true;
}
if(controller->Button.A) {
if(dz3==true) {
if(dz3i==false) {
solenoid.solenoid3 = SOLENOID_ON;
dz3i=true;
} else {
solenoid.solenoid3 = SOLENOID_OFF;
dz3i=false;
}
dz3=false;
}
} else {
dz3=true;
}
if(controller->Button.B) {
if(dz4==true) {
if(dz4i==false) {
solenoid.solenoid4 = SOLENOID_ON;
dz4i=true;
} else {
solenoid.solenoid4 = SOLENOID_OFF;
dz4i=false;
}
dz4=false;
}
} else {
dz4=true;
}
}
#endif
#if USE_PROCESS_NUM>2
static void Process2()
{
//Oh So 青ゾーン
LED::LED::TapeledMode(2);
countss++;
if(mode==1) {//スタートゾーンから白線検知
fast=40;
normal=30;
slow=20;
motor[TIRE_FR].pwm = normal;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = normal;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = normal;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = normal;
motor[TIRE_BL].dir = BACK;
if(g[2]==0&&countss>=100000) {
countss=100000;
cross=0;
mode=3;
}
} else if(mode==3) { //横ライントレースから縦ライントレースへ
motor[TIRE_FR].pwm = 30;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = 0;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BR].pwm = 0;
motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].pwm = 30;
motor[TIRE_BL].dir = BACK;
if(g[1]==0) {
mode=10;
countss=100000;
}
} else if(mode==10) { //縦ライントレース
switch(g[1]) {
case 98:
motor[TIRE_FR].pwm = normal;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = normal;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = normal;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = normal;
motor[TIRE_BL].dir = BACK;
mtc=true;
if(countss>80000) {
cross++;
countss=0;
}
if(cross==3) {
mode=11;
countss=0;
cross=0;
}
break;
case 0:
motor[TIRE_FR].pwm = fast;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = fast;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = fast;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = fast;
motor[TIRE_BL].dir = BACK;
mtc=true;
break;
case 255:
motor[TIRE_FR].pwm = fast;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = slow;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = slow;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = fast;
motor[TIRE_BL].dir = BACK;
mtc=true;
break;
case 253:
motor[TIRE_FR].pwm = slow;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = 0;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BR].pwm = 0;
motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].pwm = slow;
motor[TIRE_BL].dir = BACK;
mtc=true;
break;
case 254:
motor[TIRE_FR].pwm = normal;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = 0;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BR].pwm = 0;
motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].pwm = normal;
motor[TIRE_BL].dir = BACK;
mtc=true;
break;
case 1:
motor[TIRE_FR].pwm = slow;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = fast;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = fast;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = slow;
motor[TIRE_BL].dir = BACK;
mtc=true;
break;
case 3:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = slow;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = slow;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
mtc=true;
break;
case 2:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = normal;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = normal;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
mtc=true;
break;
}
if(g[0]!=98&&mtc==true) {
switch(g[0]) {
case 0:
mtc=false;
break;
case 255:
motor[TIRE_FL].pwm += 5;
mtc=false;
break;
case 253:
motor[TIRE_FL].pwm += 10;
mtc=false;
break;
case 254:
motor[TIRE_FL].pwm += 20;
motor[TIRE_FR].pwm = 0;
mtc=false;
break;
case 1:
motor[TIRE_FR].pwm += 5;
mtc=false;
break;
case 3:
motor[TIRE_FR].pwm += 10;
mtc=false;
break;
case 2:
motor[TIRE_FR].pwm += 20;
motor[TIRE_FL].pwm = 0;
mtc=false;
break;
}
}
} else if(mode==11) {
motor[TIRE_FR].pwm = normal;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = 0;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BR].pwm = 0;
motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].pwm = normal;
motor[TIRE_BL].dir = BACK;
if(g[2]==0) {
countss=0;
cross=0;
mode=20;
}
} else if(mode==20) {
solenoid.solenoid4= SOLENOID_ON;
switch(g[2]) {
case 98:
motor[TIRE_FR].pwm = normal;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = normal;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = normal;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = normal;
motor[TIRE_BL].dir = BACK;
if(countss>30000) {
cross++;
countss=0;
}
mtc2=true;
if(cross==1) {
fast=30;
normal=20;
slow=20;
}
if(YELLOW) {
if(cross==2) {
mode=21;
countss=0;
cross=0;
}
} else {
if(cross==1) {
mode=21;
countss=0;
cross=0;
}
}
break;
case 0:
motor[TIRE_FR].pwm = fast;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = fast;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = fast;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = fast;
motor[TIRE_BL].dir = BACK;
mtc2=true;
break;
case 255:
motor[TIRE_FR].pwm = normal;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = fast;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = fast;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = normal;
motor[TIRE_BL].dir = BACK;
mtc2=true;
break;
case 253:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = slow;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = slow;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
mtc2=true;
break;
case 254:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = normal;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = normal;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
mtc2=true;
break;
case 1:
motor[TIRE_FR].pwm = fast;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = slow;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = slow;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = fast;
motor[TIRE_BL].dir = BACK;
mtc2=true;
break;
case 3:
motor[TIRE_FR].pwm = slow;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = 0;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BR].pwm = 0;
motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].pwm = slow;
motor[TIRE_BL].dir = BACK;
mtc2=true;
break;
case 2:
motor[TIRE_FR].pwm = normal;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = 0;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BR].pwm = 0;
motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].pwm = normal;
motor[TIRE_BL].dir = BACK;
mtc2=true;
break;
}
if(g[2]!=98&&mtc2==true) {
switch(g[3]) {
case 0:
mtc2=false;
break;
case 255:
motor[TIRE_BL].pwm += 5;
mtc2=false;
break;
case 253:
motor[TIRE_BL].pwm += 10;
mtc2=false;
break;
case 254:
motor[TIRE_BL].pwm += 20;
motor[TIRE_FL].pwm = 0;
mtc2=false;
break;
case 1:
motor[TIRE_FL].pwm += 5;
mtc2=false;
break;
case 3:
motor[TIRE_FL].pwm += 10;
mtc2=false;
break;
case 2:
motor[TIRE_FL].pwm += 20;
motor[TIRE_BL].pwm = 0;
mtc2=false;
break;
}
}
}
if(YELLOW) {
if(mode==21) {
motor[TIRE_FR].pwm = 20;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 20;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = 20;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = 20;
motor[TIRE_BL].dir = FOR;
if(g[3]==98&&countss>=10000) {
mode=22;
countss=0;
fast=60;
normal=40;
slow=20;
}
} else if(mode==22) {
if(g[4]==3||g[4]==2) {
motor[TIRE_FR].pwm = 30;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = 30;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = 30;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = 30;
motor[TIRE_BL].dir = BACK;
}
if(g[4]==253||g[4]==254) {
motor[TIRE_FR].pwm = 30;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 30;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = 30;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = 30;
motor[TIRE_BL].dir = FOR;
}
switch(g[2]) {
case 0:
motor[TIRE_FR].pwm = 20;
motor[TIRE_FR].dir = FREE;
motor[TIRE_BR].pwm = 20;
motor[TIRE_BR].dir = FREE;
break;
case 255:
motor[TIRE_FR].pwm = 30;
motor[TIRE_FR].dir = BACK;
motor[TIRE_BR].pwm = 20;
motor[TIRE_BR].dir = FREE;
break;
case 253:
motor[TIRE_FR].pwm = 40;
motor[TIRE_FR].dir = BACK;
motor[TIRE_BR].pwm = 20;
motor[TIRE_BR].dir = FREE;
break;
case 254:
motor[TIRE_FR].pwm = 30;
motor[TIRE_FR].dir = BACK;
motor[TIRE_BR].pwm = 30;
motor[TIRE_BR].dir = BACK;
break;
case 1:
motor[TIRE_FR].pwm = 20;
motor[TIRE_FR].dir = FREE;
motor[TIRE_BR].pwm = 30;
motor[TIRE_BR].dir = FOR;
break;
case 3:
motor[TIRE_FR].pwm = 20;
motor[TIRE_FR].dir = FREE;
motor[TIRE_BR].pwm = 40;
motor[TIRE_BR].dir = FOR;
break;
case 2:
motor[TIRE_FR].pwm = 30;
motor[TIRE_FR].dir = FOR;
motor[TIRE_BR].pwm = 30;
motor[TIRE_BR].dir = FOR;
break;
}
switch(g[3]) {
case 0:
motor[TIRE_FL].pwm = 20;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BL].pwm = 20;
motor[TIRE_BL].dir = FREE;
break;
case 255:
motor[TIRE_FL].pwm = 20;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BL].pwm = 30;
motor[TIRE_BL].dir = BACK;
case 253:
motor[TIRE_FL].pwm = 20;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BL].pwm = 40;
motor[TIRE_BL].dir = BACK;
break;
case 254:
motor[TIRE_FL].pwm = 30;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BL].pwm = 30;
motor[TIRE_BL].dir = BACK;
break;
case 1:
motor[TIRE_FL].pwm = 30;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BL].pwm = 20;
motor[TIRE_BL].dir = FREE;
break;
case 3:
motor[TIRE_FL].pwm = 40;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BL].pwm = 20;
motor[TIRE_BL].dir = FREE;
break;
case 2:
motor[TIRE_FL].pwm = 30;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BL].pwm = 30;
motor[TIRE_BL].dir = FOR;
break;
}
if(g[3]==0&&g[2]==0) {
mode=23;
countss=0;
}
} else if(mode==23) {
switch(g[4]) {
case 98:
motor[TIRE_FR].pwm = fast;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = fast;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = fast;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = fast-5;
motor[TIRE_BL].dir = FOR;
break;
case 99:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = 0;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BR].pwm = 0;
motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
break;
case 0:
motor[TIRE_FR].pwm = fast;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = fast;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = fast;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = fast-5;
motor[TIRE_BL].dir = FOR;
break;
case 255:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = 0;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BR].pwm = 0;
motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
break;
case 253:
motor[TIRE_FR].pwm = fast;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = slow;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = slow;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = fast-5;
motor[TIRE_BL].dir = FOR;
break;
case 254:
motor[TIRE_FR].pwm = fast;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = normal;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = normal;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = fast-5;
motor[TIRE_BL].dir = FOR;
break;
case 1:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = 0;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BR].pwm = 0;
motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
break;
case 3:
motor[TIRE_FR].pwm = slow;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = fast;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = fast;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = slow-5;
motor[TIRE_BL].dir = FOR;
break;
case 2:
motor[TIRE_FR].pwm = normal;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = fast;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = fast;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = normal-5;
motor[TIRE_BL].dir = FOR;
break;
}
if(DOLS==true||UPLS==true) {
motor[MOTOR_0].pwm = 200;
motor[MOTOR_0].dir = BRAKE;
}
if(g[0]==98) {
fast=30;
normal=20;
slow=20;
if(DOLS==true||UPLS==true) {
motor[MOTOR_0].pwm = 200;
motor[MOTOR_0].dir = BRAKE;
} else {
motor[MOTOR_0].pwm = 100;
motor[MOTOR_0].dir = BACK;
}
}
if(g[2]==254&&countss>=100000) {
mode=24;
solenoid.solenoid4 = SOLENOID_OFF;
countss=0;
fast=60;
normal=40;
slow=30;
}
} else if(mode==24) {
if(DOLS==true||UPLS==true) {
motor[MOTOR_0].pwm = 200;
motor[MOTOR_0].dir = BRAKE;
} else {
motor[MOTOR_0].pwm = 100;
motor[MOTOR_0].dir = BACK;
}
if(g[4]==3||g[4]==2) {
motor[TIRE_FR].pwm = 30;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = 30;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = 30;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = 30;
motor[TIRE_BL].dir = BACK;
}
if(g[4]==253||g[4]==254) {
motor[TIRE_FR].pwm = 30;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 30;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = 30;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = 30;
motor[TIRE_BL].dir = FOR;
}
switch(g[2]) {
case 0:
motor[TIRE_FR].pwm = 20;
motor[TIRE_FR].dir = FREE;
motor[TIRE_BR].pwm = 20;
motor[TIRE_BR].dir = FREE;
break;
case 255:
motor[TIRE_FR].pwm = 30;
motor[TIRE_FR].dir = BACK;
motor[TIRE_BR].pwm = 20;
motor[TIRE_BR].dir = FREE;
break;
case 253:
motor[TIRE_FR].pwm = 40;
motor[TIRE_FR].dir = BACK;
motor[TIRE_BR].pwm = 20;
motor[TIRE_BR].dir = FREE;
break;
case 254:
motor[TIRE_FR].pwm = 30;
motor[TIRE_FR].dir = BACK;
motor[TIRE_BR].pwm = 30;
motor[TIRE_BR].dir = BACK;
break;
case 1:
motor[TIRE_FR].pwm = 20;
motor[TIRE_FR].dir = FREE;
motor[TIRE_BR].pwm = 30;
motor[TIRE_BR].dir = FOR;
break;
case 3:
motor[TIRE_FR].pwm = 20;
motor[TIRE_FR].dir = FREE;
motor[TIRE_BR].pwm = 40;
motor[TIRE_BR].dir = FOR;
break;
case 2:
motor[TIRE_FR].pwm = 30;
motor[TIRE_FR].dir = FOR;
motor[TIRE_BR].pwm = 30;
motor[TIRE_BR].dir = FOR;
break;
}
switch(g[3]) {
case 0:
motor[TIRE_FL].pwm = 20;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BL].pwm = 20;
motor[TIRE_BL].dir = FREE;
break;
case 255:
motor[TIRE_FL].pwm = 20;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BL].pwm = 30;
motor[TIRE_BL].dir = BACK;
case 253:
motor[TIRE_FL].pwm = 20;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BL].pwm = 40;
motor[TIRE_BL].dir = BACK;
break;
case 254:
motor[TIRE_FL].pwm = 30;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BL].pwm = 30;
motor[TIRE_BL].dir = BACK;
break;
case 1:
motor[TIRE_FL].pwm = 30;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BL].pwm = 20;
motor[TIRE_BL].dir = FREE;
break;
case 3:
motor[TIRE_FL].pwm = 40;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BL].pwm = 20;
motor[TIRE_BL].dir = FREE;
break;
case 2:
motor[TIRE_FL].pwm = 30;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BL].pwm = 30;
motor[TIRE_BL].dir = FOR;
break;
}
if(g[3]==0&&g[2]==0) {
if(UPLS==true||DOLS==true) {
motor[MOTOR_0].pwm = 200;
motor[MOTOR_0].dir = BRAKE;
mode=25;
} else {
motor[MOTOR_0].pwm = 100;
motor[MOTOR_0].dir = BACK;
}
}
} else if(mode==25) {
switch(g[4]) {
case 98:
motor[TIRE_FR].pwm = fast;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = fast;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = fast;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = fast-5;
motor[TIRE_BL].dir = FOR;
break;
case 99:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = 0;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BR].pwm = 0;
motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
break;
case 0:
motor[TIRE_FR].pwm = fast;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = fast;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = fast;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = fast-5;
motor[TIRE_BL].dir = FOR;
break;
case 255:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = 0;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BR].pwm = 0;
motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
break;
case 253:
motor[TIRE_FR].pwm = fast;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = slow;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = slow;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = fast-5;
motor[TIRE_BL].dir = FOR;
break;
case 254:
motor[TIRE_FR].pwm = fast;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = normal;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = normal;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = fast-5;
motor[TIRE_BL].dir = FOR;
break;
case 1:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = 0;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BR].pwm = 0;
motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
break;
case 3:
motor[TIRE_FR].pwm = slow;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = fast;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = fast;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = slow-5;
motor[TIRE_BL].dir = FOR;
break;
case 2:
motor[TIRE_FR].pwm = normal;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = fast;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = fast;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = normal-5;
motor[TIRE_BL].dir = FOR;
break;
}
if(g[1]==98) {
mode=31;
}
}
} else {
if(mode==21) {
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = 30;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = 30;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
if(g[1]==0) {
mode=22;
fast=60;
normal=40;
slow=20;
}
} else if(mode==22) {
if(g[3]==0) {
mode=23;
}
switch(g[0]) {
case 98:
solenoid.solenoid4 = SOLENOID_OFF;
fast=30;
normal=20;
slow=20;
motor[TIRE_FR].pwm = normal;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = normal;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = normal;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = normal;
motor[TIRE_BL].dir = FOR;
break;
case 0:
motor[TIRE_FR].pwm = fast;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = fast;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = fast;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = fast;
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
case 255:
motor[TIRE_FR].pwm = fast;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = slow;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = slow;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = fast;
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
case 253:
motor[TIRE_FR].pwm = slow;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 0;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BR].pwm = 0;
motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].pwm = slow;
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
case 254:
motor[TIRE_FR].pwm = normal;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 0;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BR].pwm = 0;
motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].pwm = normal;
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
case 1:
motor[TIRE_FR].pwm = slow;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = fast;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = fast;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = slow;
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
case 3:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = slow;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = slow;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
mtc=true;
break;
case 2:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = normal;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = normal;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
mtc=true;
break;
}
if(g[0]!=98&&mtc==true) {
switch(g[0]) {
case 0:
mtc=false;
break;
case 255:
motor[TIRE_BR].pwm += 5;
mtc=false;
break;
case 253:
motor[TIRE_BR].pwm += 10;
mtc=false;
break;
case 254:
motor[TIRE_BR].pwm += 20;
motor[TIRE_BL].pwm = 0;
mtc=false;
break;
case 1:
motor[TIRE_BL].pwm += 5;
mtc=false;
break;
case 3:
motor[TIRE_BL].pwm += 10;
mtc=false;
break;
case 2:
motor[TIRE_BL].pwm += 20;
motor[TIRE_BR].pwm = 0;
mtc=false;
break;
}
}
} else if(mode==23) {
if(UPLS==true||DOLS==true) {
motor[MOTOR_0].pwm = 200;
motor[MOTOR_0].dir = BRAKE;
fast=60;
normal=40;
slow=20;
countss=0;
mode=24;
} else {
motor[MOTOR_0].pwm = 100;
motor[MOTOR_0].dir = BACK;
motor[TIRE_FR].pwm = 100;
motor[TIRE_FR].dir = BRAKE;
motor[TIRE_FL].pwm = 100;
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_BR].pwm = 100;
motor[TIRE_BR].dir = BRAKE;
motor[TIRE_BL].pwm = 100;
motor[TIRE_BL].dir = BRAKE;
}
} else if(mode==24) {
if(g[0]==98&&countss>=40000) {
mode=31;
countss=0;
}
switch(g[0]) {
case 98:
fast=30;
normal=20;
slow=20;
motor[TIRE_FR].pwm = normal;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = normal;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = normal;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = normal;
motor[TIRE_BL].dir = FOR;
break;
case 0:
motor[TIRE_FR].pwm = fast;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = fast;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = fast;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = fast;
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
case 255:
motor[TIRE_FR].pwm = fast;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = slow;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = slow;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = fast;
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
case 253:
motor[TIRE_FR].pwm = slow;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 0;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BR].pwm = 0;
motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].pwm = slow;
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
case 254:
motor[TIRE_FR].pwm = normal;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 0;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BR].pwm = 0;
motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].pwm = normal;
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
case 1:
motor[TIRE_FR].pwm = slow;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = fast;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = fast;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = slow;
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
case 3:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = slow;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = slow;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
mtc=true;
break;
case 2:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = normal;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = normal;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
mtc=true;
break;
}
if(g[1]!=98&&mtc==true) {
switch(g[1]) {
case 0:
mtc=false;
break;
case 255:
motor[TIRE_BR].pwm += 5;
mtc=false;
break;
case 253:
motor[TIRE_BR].pwm += 10;
mtc=false;
break;
case 254:
motor[TIRE_BR].pwm += 20;
motor[TIRE_BL].pwm = 0;
mtc=false;
break;
case 1:
motor[TIRE_BL].pwm += 5;
mtc=false;
break;
case 3:
motor[TIRE_BL].pwm += 10;
mtc=false;
break;
case 2:
motor[TIRE_BL].pwm += 20;
motor[TIRE_BR].pwm = 0;
mtc=false;
break;
}
}
}
}
if(mode==31) {
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = 0;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BR].pwm = 0;
motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
if(countss>=5000) {
countss=0;
mode=33;
}
} else if(mode==33) {
motor[TIRE_FR].pwm = 30;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 0;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BR].pwm = 0;
motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].pwm = 30;
motor[TIRE_BL].dir = FOR;
if(g[3]==0) {
cross=0;
countss=0;
mode=40;
}
} else if(mode==40) {
switch(g[3]) {
case 98:
motor[TIRE_FR].pwm = normal;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = normal;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = normal;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = normal;
motor[TIRE_BL].dir = FOR;
if(countss>10000) {
cross++;
countss=0;
}
mtc2=true;
if(cross==1) {
mode=41;
countss=0;
cross=0;
}
break;
case 0:
motor[TIRE_FR].pwm = fast;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = fast;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = fast;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = fast;
motor[TIRE_BL].dir = FOR;
mtc2=true;
break;
case 255:
motor[TIRE_FR].pwm = slow;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = fast;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = fast;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = slow;
motor[TIRE_BL].dir = FOR;
mtc2=true;
break;
case 253:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = slow;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = slow;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
mtc2=true;
break;
case 254:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = normal;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = normal;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
mtc2=true;
break;
case 1:
motor[TIRE_FR].pwm = fast;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = slow;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = slow;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = fast;
motor[TIRE_BL].dir = FOR;
mtc2=true;
break;
case 3:
motor[TIRE_FR].pwm = slow;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 0;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BR].pwm = 0;
motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].pwm = slow;
motor[TIRE_BL].dir = FOR;
mtc2=true;
break;
case 2:
motor[TIRE_FR].pwm = normal;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 0;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BR].pwm = 0;
motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].pwm = normal;
motor[TIRE_BL].dir = FOR;
mtc2=true;
break;
}
if(g[2]!=98&&mtc2==true) {
switch(g[2]) {
case 0:
mtc2=false;
break;
case 255:
motor[TIRE_FL].pwm += 5;
mtc2=false;
break;
case 253:
motor[TIRE_FL].pwm += 10;
mtc2=false;
break;
case 254:
motor[TIRE_FL].pwm += 20;
motor[TIRE_BL].pwm = 0;
mtc2=false;
break;
case 1:
motor[TIRE_BL].pwm += 5;
mtc2=false;
break;
case 3:
motor[TIRE_BL].pwm += 10;
mtc2=false;
break;
case 2:
motor[TIRE_BL].pwm += 20;
motor[TIRE_FL].pwm = 0;
mtc2=false;
break;
}
}
} else if(mode==41) {
if(LIF||LIB) {
mode=42;
}
motor[TIRE_FR].pwm = 30;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 0;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BR].pwm = 0;
motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].pwm = 30;
motor[TIRE_BL].dir = FOR;
if(g[1]==0) {
countss=0;
mode=70;
normal=60;
fast=70;
slow=40;
}
} else if(mode==42) {
motor[TIRE_FR].pwm = 20;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = 0;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BR].pwm = 0;
motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].pwm = 20;
motor[TIRE_BL].dir = BACK;
if(g[1]==0) {
countss=0;
mode=70;
}
} else if(mode==70) {
switch(g[0]) {
case 99:
if(countss>=100000) {
mode=100;
}
break;
case 98:
motor[TIRE_FR].pwm = normal;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = normal;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = normal;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = normal;
motor[TIRE_BL].dir = FOR;
break;
case 0:
motor[TIRE_FR].pwm = 80;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 80;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = 80;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = 80;
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
case 255:
motor[TIRE_FR].pwm = fast;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = slow;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = slow;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = fast;
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
case 253:
motor[TIRE_FR].pwm = slow;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 0;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BR].pwm = 0;
motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].pwm = slow;
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
case 254:
motor[TIRE_FR].pwm = normal;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 0;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BR].pwm = 0;
motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].pwm = normal;
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
case 1:
motor[TIRE_FR].pwm = slow;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = fast;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = fast;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = slow;
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
case 3:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = slow;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = slow;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
mtc=true;
break;
case 2:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = normal;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = normal;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
mtc=true;
break;
}
if(g[1]!=98&&mtc==true) {
switch(g[1]) {
case 0:
mtc=false;
break;
case 255:
motor[TIRE_BR].pwm += 5;
mtc=false;
break;
case 253:
motor[TIRE_BR].pwm += 10;
mtc=false;
break;
case 254:
motor[TIRE_BR].pwm += 20;
motor[TIRE_BL].pwm = 0;
mtc=false;
break;
case 1:
motor[TIRE_BL].pwm += 5;
mtc=false;
break;
case 3:
motor[TIRE_BL].pwm += 10;
mtc=false;
break;
case 2:
motor[TIRE_BL].pwm += 20;
motor[TIRE_BR].pwm = 0;
mtc=false;
break;
}
}
} else if(mode==100) {
motor[TIRE_FR].pwm = 40;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 40;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = 43;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = 40;
motor[TIRE_BL].dir = FOR;
} else {
}
}
#endif
#if USE_PROCESS_NUM>3
static void Process3()
{
//Clearly 赤ゾーン
LED::LED::TapeledMode(3);
countss++;
if(mode==1) {//スタートゾーンから白線検知
fast=40;
normal=30;
slow=20;
motor[TIRE_FR].pwm = normal+20;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = normal+20;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = normal+20;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = normal+20;
motor[TIRE_BL].dir = FOR;
if(g[2]==0&&countss>=100000) {
countss=100000;
cross=0;
mode=3;
}
}
else if(mode==3) { //横ライントレースから縦ライントレースへ
motor[TIRE_FR].pwm = 255;
motor[TIRE_FR].dir = BRAKE;
motor[TIRE_FL].pwm = 30;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = 30;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = 255;
motor[TIRE_BL].dir = BRAKE;
if(g[0]==0) {
mode=10;
countss=100000;
}
}
else if(mode==10) { //縦ライントレース
switch(g[0]) {
case 98:
motor[TIRE_FR].pwm = normal+50;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = normal+50;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = normal+50;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = normal+50;
motor[TIRE_BL].dir = FOR;
mtc=true;
if(countss>80000) {
cross++;
countss=0;
}
if(cross==2){
fast = 10;
normal = 10;
slow = 5;
}
if(cross==3) {
fast=40;
normal=30;
slow=20;
mode=11;
countss=0;
cross=0;
}
break;
case 0:
motor[TIRE_FR].pwm = fast+50;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = fast+50;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = fast+50;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = fast+50;
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
case 255:
motor[TIRE_FR].pwm = fast+50;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = slow+50;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = slow+50;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = fast+50;
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
case 253:
motor[TIRE_FR].pwm = slow+50;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 0;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BR].pwm = 0;
motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].pwm = slow+50;
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
case 254:
motor[TIRE_FR].pwm = normal+50;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 0;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BR].pwm = 0;
motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].pwm = normal+50;
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
case 1:
motor[TIRE_FR].pwm = slow+50;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = fast+50;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = fast+50;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = slow+50;
motor[TIRE_BL].dir = FOR;
mtc=true;
break;
case 3:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = slow+50;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = slow+50;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
mtc=true;
break;
case 2:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = normal+50;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = normal+50;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
mtc=true;
break;
}
if(g[0]!=98&&mtc==true) {
switch(g[1]) {
case 0:
mtc=false;
break;
case 255:
motor[TIRE_BR].pwm += 3;
mtc=false;
break;
case 253:
motor[TIRE_BR].pwm += 6;
mtc=false;
break;
case 254:
motor[TIRE_BR].pwm += 9;
motor[TIRE_BL].pwm = 0;
mtc=false;
break;
case 1:
motor[TIRE_BL].pwm += 3;
mtc=false;
break;
case 3:
motor[TIRE_BL].pwm += 6;
mtc=false;
break;
case 2:
motor[TIRE_BL].pwm += 9;
motor[TIRE_BR].pwm = 0;
mtc=false;
break;
}
}
}
else if(mode==11) {
motor[TIRE_FR].pwm = 25;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = normal;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = normal;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = 25;
motor[TIRE_BL].dir = BACK;
if(g[2]==0 || g[2]==255 || g[2]==254 || g[2]==253) {
countss=0;
cross=0;
mode=20;
}
}
else if(mode==20) {
solenoid.solenoid4= SOLENOID_ON;
switch(g[2]) {
case 98:
motor[TIRE_FR].pwm = normal+20;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = normal+20;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = normal+20;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = normal+20;
motor[TIRE_BL].dir = BACK;
if(countss>30000) {
cross++;
countss=0;
}
mtc2=true;
if(YELLOW) {
if(cross==1) {
fast=30;
normal=20;
slow=20;
}
else if(cross==2) {
mode=21;
countss=0;
cross=0;
}
} else {
if(cross==3) {
mode=21;
countss=0;
cross=0;
}
}
break;
case 0:
motor[TIRE_FR].pwm = fast+20;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = fast+20;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = fast+20;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = fast+20;
motor[TIRE_BL].dir = BACK;
mtc2=true;
break;
case 255:
motor[TIRE_FR].pwm = normal+20;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = fast+20;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = fast+20;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = normal+20;
motor[TIRE_BL].dir = BACK;
mtc2=true;
break;
case 253:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = slow+20;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = slow+20;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
mtc2=true;
break;
case 254:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = normal+20;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = normal+20;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
mtc2=true;
break;
case 1:
motor[TIRE_FR].pwm = fast+20;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = slow+20;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = slow+20;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = fast+20;
motor[TIRE_BL].dir = BACK;
mtc2=true;
break;
case 3:
motor[TIRE_FR].pwm = slow+20;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = 0;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BR].pwm = 0;
motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].pwm = slow+20;
motor[TIRE_BL].dir = BACK;
mtc2=true;
break;
case 2:
motor[TIRE_FR].pwm = normal+20;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = 0;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BR].pwm = 0;
motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].pwm = normal+20;
motor[TIRE_BL].dir = BACK;
mtc2=true;
break;
}
if(g[2]!=98&&mtc2==true) {
switch(g[3]) {
case 0:
mtc2=false;
break;
case 255:
motor[TIRE_BL].pwm += 5;
mtc2=false;
break;
case 253:
motor[TIRE_BL].pwm += 10;
mtc2=false;
break;
case 254:
motor[TIRE_BL].pwm += 15;
motor[TIRE_FL].pwm = 0;
mtc2=false;
break;
case 1:
motor[TIRE_FL].pwm += 5;
mtc2=false;
break;
case 3:
motor[TIRE_FL].pwm += 10;
mtc2=false;
break;
case 2:
motor[TIRE_FL].pwm += 15;
motor[TIRE_BL].pwm = 0;
mtc2=false;
break;
}
}
}
if(YELLOW) {
if(mode==21) {
motor[TIRE_FR].pwm = 20;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 20;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = 20;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = 20;
motor[TIRE_BL].dir = FOR;
if(g[3]==98&&countss>=10000) {
mode=22;
countss=0;
fast=60;
normal=40;
slow=20;
}
}
else if(mode==22) {
if(g[4]==3||g[4]==2) {
motor[TIRE_FR].pwm = 30;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = 30;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = 30;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = 30;
motor[TIRE_BL].dir = BACK;
}
if(g[4]==253||g[4]==254) {
motor[TIRE_FR].pwm = 30;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 30;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = 30;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = 30;
motor[TIRE_BL].dir = FOR;
}
switch(g[2]) {
case 0:
motor[TIRE_FR].pwm = 20;
motor[TIRE_FR].dir = FREE;
motor[TIRE_BR].pwm = 20;
motor[TIRE_BR].dir = FREE;
break;
case 255:
motor[TIRE_FR].pwm = 30;
motor[TIRE_FR].dir = BACK;
motor[TIRE_BR].pwm = 20;
motor[TIRE_BR].dir = FREE;
break;
case 253:
motor[TIRE_FR].pwm = 40;
motor[TIRE_FR].dir = BACK;
motor[TIRE_BR].pwm = 20;
motor[TIRE_BR].dir = FREE;
break;
case 254:
motor[TIRE_FR].pwm = 30;
motor[TIRE_FR].dir = BACK;
motor[TIRE_BR].pwm = 30;
motor[TIRE_BR].dir = BACK;
break;
case 1:
motor[TIRE_FR].pwm = 20;
motor[TIRE_FR].dir = FREE;
motor[TIRE_BR].pwm = 30;
motor[TIRE_BR].dir = FOR;
break;
case 3:
motor[TIRE_FR].pwm = 20;
motor[TIRE_FR].dir = FREE;
motor[TIRE_BR].pwm = 40;
motor[TIRE_BR].dir = FOR;
break;
case 2:
motor[TIRE_FR].pwm = 30;
motor[TIRE_FR].dir = FOR;
motor[TIRE_BR].pwm = 30;
motor[TIRE_BR].dir = FOR;
break;
}
switch(g[3]) {
case 0:
motor[TIRE_FL].pwm = 20;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BL].pwm = 20;
motor[TIRE_BL].dir = FREE;
break;
case 255:
motor[TIRE_FL].pwm = 20;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BL].pwm = 30;
motor[TIRE_BL].dir = BACK;
case 253:
motor[TIRE_FL].pwm = 20;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BL].pwm = 40;
motor[TIRE_BL].dir = BACK;
break;
case 254:
motor[TIRE_FL].pwm = 30;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BL].pwm = 30;
motor[TIRE_BL].dir = BACK;
break;
case 1:
motor[TIRE_FL].pwm = 30;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BL].pwm = 20;
motor[TIRE_BL].dir = FREE;
break;
case 3:
motor[TIRE_FL].pwm = 40;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BL].pwm = 20;
motor[TIRE_BL].dir = FREE;
break;
case 2:
motor[TIRE_FL].pwm = 30;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BL].pwm = 30;
motor[TIRE_BL].dir = FOR;
break;
}
if(g[3]==0&&g[2]==0) {
mode=23;
countss=0;
}
}
else if(mode==23) {
switch(g[4]) {
case 98:
motor[TIRE_FR].pwm = fast-5;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = fast-5;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = fast-5;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = fast-5;
motor[TIRE_BL].dir = BACK;
break;
case 99:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = 0;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BR].pwm = 0;
motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
break;
case 0:
motor[TIRE_FR].pwm = fast-5;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = fast-5;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = fast-5;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = fast-5;
motor[TIRE_BL].dir = BACK;
break;
case 255:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = 0;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BR].pwm = 0;
motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
break;
case 253:
motor[TIRE_FR].pwm = slow-5;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = fast-5;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = fast-5;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = slow-5;
motor[TIRE_BL].dir = BACK;
break;
case 254:
motor[TIRE_FR].pwm = normal-5;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = fast-5;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = fast-5;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = normal-5;
motor[TIRE_BL].dir = BACK;
break;
case 1:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = 0;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BR].pwm = 0;
motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
break;
case 3:
motor[TIRE_FR].pwm = fast-5;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = slow-5;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = slow-5;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = fast-5;
motor[TIRE_BL].dir = BACK;
break;
case 2:
motor[TIRE_FR].pwm = fast-5;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = normal-5;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = normal-5;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = fast-5;
motor[TIRE_BL].dir = BACK;
break;
}
if(DOLS==true||UPLS==true) {
motor[MOTOR_0].pwm = 200;
motor[MOTOR_0].dir = BRAKE;
}
if(g[1]==98) {
fast=20;
normal=15;
slow=10;
if(DOLS==true||UPLS==true) {
motor[MOTOR_0].pwm = 200;
motor[MOTOR_0].dir = BRAKE;
} else {
motor[MOTOR_0].pwm = 100;
motor[MOTOR_0].dir = BACK;
}
}
if(g[3]==254&&countss>=100000) {
mode=24;
solenoid.solenoid4 = SOLENOID_OFF;
countss=0;
fast=60;
normal=40;
slow=20;
}
}
else if(mode==24) {
if(DOLS==true||UPLS==true) {
motor[MOTOR_0].pwm = 200;
motor[MOTOR_0].dir = BRAKE;
}
else {
motor[MOTOR_0].pwm = 100;
motor[MOTOR_0].dir = BACK;
}
if(g[4]==3||g[4]==2) {
motor[TIRE_FR].pwm = 30;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = 30;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].pwm = 30;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].pwm = 30;
motor[TIRE_BL].dir = BACK;
}
if(g[4]==253||g[4]==254) {
motor[TIRE_FR].pwm = 30;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 30;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = 30;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = 30;
motor[TIRE_BL].dir = FOR;
}
switch(g[2]) {
case 0:
motor[TIRE_FR].pwm = 20;
motor[TIRE_FR].dir = FREE;
motor[TIRE_BR].pwm = 20;
motor[TIRE_BR].dir = FREE;
break;
case 255:
motor[TIRE_FR].pwm = 30;
motor[TIRE_FR].dir = BACK;
motor[TIRE_BR].pwm = 20;
motor[TIRE_BR].dir = FREE;
break;
case 253:
motor[TIRE_FR].pwm = 40;
motor[TIRE_FR].dir = BACK;
motor[TIRE_BR].pwm = 20;
motor[TIRE_BR].dir = FREE;
break;
case 254:
motor[TIRE_FR].pwm = 30;
motor[TIRE_FR].dir = BACK;
motor[TIRE_BR].pwm = 30;
motor[TIRE_BR].dir = BACK;
break;
case 1:
motor[TIRE_FR].pwm = 20;
motor[TIRE_FR].dir = FREE;
motor[TIRE_BR].pwm = 30;
motor[TIRE_BR].dir = FOR;
break;
case 3:
motor[TIRE_FR].pwm = 20;
motor[TIRE_FR].dir = FREE;
motor[TIRE_BR].pwm = 40;
motor[TIRE_BR].dir = FOR;
break;
case 2:
motor[TIRE_FR].pwm = 30;
motor[TIRE_FR].dir = FOR;
motor[TIRE_BR].pwm = 30;
motor[TIRE_BR].dir = FOR;
break;
}
switch(g[3]) {
case 0:
motor[TIRE_FL].pwm = 20;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BL].pwm = 20;
motor[TIRE_BL].dir = FREE;
break;
case 255:
motor[TIRE_FL].pwm = 20;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BL].pwm = 30;
motor[TIRE_BL].dir = BACK;
case 253:
motor[TIRE_FL].pwm = 20;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BL].pwm = 40;
motor[TIRE_BL].dir = BACK;
break;
case 254:
motor[TIRE_FL].pwm = 30;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BL].pwm = 30;
motor[TIRE_BL].dir = BACK;
break;
case 1:
motor[TIRE_FL].pwm = 30;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BL].pwm = 20;
motor[TIRE_BL].dir = FREE;
break;
case 3:
motor[TIRE_FL].pwm = 40;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BL].pwm = 20;
motor[TIRE_BL].dir = FREE;
break;
case 2:
motor[TIRE_FL].pwm = 30;
motor[TIRE_FL].dir = FOR;
motor[TIRE_BL].pwm = 30;
motor[TIRE_BL].dir = FOR;
break;
}
if(g[3]==0&&g[2]==0) {
if(UPLS==true||DOLS==true) {
motor[MOTOR_0].pwm = 200;
motor[MOTOR_0].dir = BRAKE;
mode=25;
} else {
motor[MOTOR_0].pwm = 100;
motor[MOTOR_0].dir = BACK;
}
}
}
else if(mode==25) {
switch(g[4]) {
case 98:
motor[TIRE_FR].pwm = 60;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = 60;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = 60;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = 60;
motor[TIRE_BL].dir = BACK;
break;
case 99:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = 0;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BR].pwm = 0;
motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
break;
case 0:
motor[TIRE_FR].pwm = 60;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = 60;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = 60;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = 60;
motor[TIRE_BL].dir = BACK;
break;
case 255:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = 0;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BR].pwm = 0;
motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
break;
case 253:
motor[TIRE_FR].pwm = 20;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = 60;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = 60;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = 20;
motor[TIRE_BL].dir = BACK;
break;
case 254:
motor[TIRE_FR].pwm = 40;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = 60;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = 60;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = 40;
motor[TIRE_BL].dir = BACK;
break;
case 1:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = 0;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BR].pwm = 0;
motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
break;
case 3:
motor[TIRE_FR].pwm = 60;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = 20;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = 20;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = 60;
motor[TIRE_BL].dir = BACK;
break;
case 2:
motor[TIRE_FR].pwm = 60;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = 40;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = 40;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = 60;
motor[TIRE_BL].dir = BACK;
break;
}
if(g[1]==98) {
mode=33;
}
}
}
else {
if(mode==21) {
motor[TIRE_FR].pwm = 30;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = 255;
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_BR].pwm = 255;
motor[TIRE_BR].dir = BRAKE;
motor[TIRE_BL].pwm = 30;
motor[TIRE_BL].dir = BACK;
if(g[1]==0) {
mode=22;
fast=60;
normal=40;
slow=20;
}
}
else if(mode==22) {
if(g[1]==98) {
slow=20;
normal=30;
fast=40;
}
if(g[3]==0){
slow=20;
normal=40;
fast=60;
mode=23;
}
switch(g[1]) {
case 98:
solenoid.solenoid4 = SOLENOID_OFF;
fast=40;
normal=30;
slow=20;
motor[TIRE_FR].pwm = normal;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = normal;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = normal;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = normal;
motor[TIRE_BL].dir = BACK;
break;
case 0:
motor[TIRE_FR].pwm = fast;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = fast;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = fast;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = fast;
motor[TIRE_BL].dir = BACK;
mtc=true;
break;
case 255:
motor[TIRE_FR].pwm = fast;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = slow;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = slow;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = fast;
motor[TIRE_BL].dir = BACK;
mtc=true;
break;
case 253:
motor[TIRE_FR].pwm = slow;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = 0;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BR].pwm = 0;
motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].pwm = slow;
motor[TIRE_BL].dir = BACK;
mtc=true;
break;
case 254:
motor[TIRE_FR].pwm = normal;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = 0;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BR].pwm = 0;
motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].pwm = normal;
motor[TIRE_BL].dir = BACK;
mtc=true;
break;
case 1:
motor[TIRE_FR].pwm = slow;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = fast;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = fast;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = slow;
motor[TIRE_BL].dir = BACK;
mtc=true;
break;
case 3:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = slow;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = slow;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
mtc=true;
break;
case 2:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = normal;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = normal;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
mtc=true;
break;
}
if(g[1]!=98&&mtc==true) {
switch(g[0]) {
case 0:
mtc=false;
break;
case 255:
motor[TIRE_FL].pwm += 3;
mtc=false;
break;
case 253:
motor[TIRE_FL].pwm += 6;
mtc=false;
break;
case 254:
motor[TIRE_FL].pwm += 9;
motor[TIRE_FR].pwm = 0;
mtc=false;
break;
case 1:
motor[TIRE_FR].pwm += 3;
mtc=false;
break;
case 3:
motor[TIRE_FR].pwm += 6;
mtc=false;
break;
case 2:
motor[TIRE_FR].pwm += 9;
motor[TIRE_FL].pwm = 0;
mtc=false;
break;
}
}
}
else if(mode==23) {
if(UPLS==true||DOLS==true) {
motor[MOTOR_0].pwm = 200;
motor[MOTOR_0].dir = BRAKE;
fast=60;
normal=40;
slow=20;
countss=0;
mode=24;
}
else {
motor[MOTOR_0].pwm = 100;
motor[MOTOR_0].dir = BACK;
}
motor[TIRE_FR].pwm = 100;
motor[TIRE_FR].dir = BRAKE;
motor[TIRE_FL].pwm = 100;
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_BR].pwm = 100;
motor[TIRE_BR].dir = BRAKE;
motor[TIRE_BL].pwm = 100;
motor[TIRE_BL].dir = BRAKE;
}
else if(mode==24) {
if(g[1]==98&&countss>=40000) {
mode=33;
countss=0;
}
switch(g[1]) {
case 98:
fast=40;
normal=30;
slow=20;
motor[TIRE_FR].pwm = normal;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = normal;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = normal;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = normal;
motor[TIRE_BL].dir = BACK;
break;
case 0:
motor[TIRE_FR].pwm = fast;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = fast;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = fast;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = fast;
motor[TIRE_BL].dir = BACK;
mtc=true;
break;
case 255:
motor[TIRE_FR].pwm = fast;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = slow;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = slow;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = fast;
motor[TIRE_BL].dir = BACK;
mtc=true;
break;
case 253:
motor[TIRE_FR].pwm = slow;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = 0;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BR].pwm = 0;
motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].pwm = slow;
motor[TIRE_BL].dir = BACK;
mtc=true;
break;
case 254:
motor[TIRE_FR].pwm = normal;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = 0;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BR].pwm = 0;
motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].pwm = normal;
motor[TIRE_BL].dir = BACK;
mtc=true;
break;
case 1:
motor[TIRE_FR].pwm = slow;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = fast;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = fast;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = slow;
motor[TIRE_BL].dir = BACK;
mtc=true;
break;
case 3:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = slow;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = slow;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
mtc=true;
break;
case 2:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = normal;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = normal;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
mtc=true;
break;
}
if(g[1]!=98&&mtc==true) {
switch(g[0]) {
case 0:
mtc=false;
break;
case 255:
motor[TIRE_FL].pwm += 3;
mtc=false;
break;
case 253:
motor[TIRE_FL].pwm += 6;
mtc=false;
break;
case 254:
motor[TIRE_FL].pwm += 9;
motor[TIRE_FR].pwm = 0;
mtc=false;
break;
case 1:
motor[TIRE_FR].pwm += 3;
mtc=false;
break;
case 3:
motor[TIRE_FR].pwm += 6;
mtc=false;
break;
case 2:
motor[TIRE_FR].pwm += 9;
motor[TIRE_FL].pwm = 0;
mtc=false;
break;
}
}
}
}
/*
if(mode==31) {
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = 0;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BR].pwm = 0;
motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
if(countss>=5000) {
countss=0;
mode=32;
}
} else if(mode==32) {
motor[TIRE_FR].pwm = 10;
motor[TIRE_FR].dir = BRAKE;
motor[TIRE_FL].pwm = 10;
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_BR].pwm = 10;
motor[TIRE_BR].dir = BRAKE;
motor[TIRE_BL].pwm = 10;
motor[TIRE_BL].dir = BRAKE;
if(countss>=40000) {
mode=33;
countss=0;
}
}
*/
if(mode==33) {
motor[TIRE_FR].pwm = 255;
motor[TIRE_FR].dir = BRAKE;
motor[TIRE_FL].pwm = 45;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = 45;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = 255;
motor[TIRE_BL].dir = BRAKE;
if(g[2]==0) {
cross=0;
countss=0;
mode=40;
}
}
else if(mode==40) {
switch(g[3]) {
case 98:
motor[TIRE_FR].pwm = normal+30;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = normal+30;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = normal+30;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = normal+30;
motor[TIRE_BL].dir = FOR;
if(countss>30000) {
cross++;
countss=0;
}
mtc2=true;
break;
case 0:
motor[TIRE_FR].pwm = fast+30;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = fast+30;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = fast+30;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = fast+30;
motor[TIRE_BL].dir = FOR;
mtc2=true;
break;
case 255:
motor[TIRE_FR].pwm = slow+30;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = fast+30;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = fast+30;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = slow+30;
motor[TIRE_BL].dir = FOR;
mtc2=true;
break;
case 253:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = slow+30;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = slow+30;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
mtc2=true;
break;
case 254:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = normal+30;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = normal+30;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
mtc2=true;
break;
case 1:
motor[TIRE_FR].pwm = fast+30;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = slow+30;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = slow+30;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = fast+30;
motor[TIRE_BL].dir = FOR;
mtc2=true;
break;
case 3:
motor[TIRE_FR].pwm = slow+30;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 0;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BR].pwm = 0;
motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].pwm = slow+30;
motor[TIRE_BL].dir = FOR;
mtc2=true;
break;
case 2:
motor[TIRE_FR].pwm = normal+30;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].pwm = 0;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BR].pwm = 0;
motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].pwm = normal+30;
motor[TIRE_BL].dir = FOR;
mtc2=true;
break;
}
if(g[2]!=98&&mtc2==true) {
switch(g[2]) {
case 0:
mtc2=false;
break;
case 255:
motor[TIRE_FR].pwm += 3;
mtc2=false;
break;
case 253:
motor[TIRE_FR].pwm += 6;
mtc2=false;
break;
case 254:
motor[TIRE_FR].pwm += 9;
motor[TIRE_BR].pwm = 0;
mtc2=false;
break;
case 1:
motor[TIRE_BR].pwm += 3;
mtc2=false;
break;
case 3:
motor[TIRE_BR].pwm += 6;
mtc2=false;
break;
case 2:
motor[TIRE_BR].pwm += 9;
motor[TIRE_FR].pwm = 0;
mtc2=false;
break;
}
}
if(YELLOW) {
if(cross==1) {
mode=41;
countss=0;
cross=0;
}
}
else {
if(cross==3) {
mode=41;
countss=0;
cross=0;
}
}
}
else if(mode==41) {
if(LIF||LIB) {
mode=42;
}
motor[TIRE_FR].pwm = 255;
motor[TIRE_FR].dir = BRAKE;
motor[TIRE_FL].pwm = 40;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = 40;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = 255;
motor[TIRE_BL].dir = BRAKE;
if(g[1]==0) {
countss=0;
mode=70;
normal=80;
fast=100;
slow=60;
}
}
else if(mode==42) {
motor[TIRE_FR].pwm = 40;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = 0;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BR].pwm = 0;
motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].pwm = 40;
motor[TIRE_BL].dir = BACK;
if(g[1]==0) {
countss=0;
mode=70;
}
}
else if(mode==70) {
switch(g[1]) {
case 99:
if(countss>=100000) {
mode=100;
}
break;
case 98:
motor[TIRE_FR].pwm = normal;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = normal;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = normal;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = normal;
motor[TIRE_BL].dir = BACK;
break;
case 0:
motor[TIRE_FR].pwm = 100;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = 100;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = 100;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = 100;
motor[TIRE_BL].dir = BACK;
mtc=true;
break;
case 255:
motor[TIRE_FR].pwm = fast;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = slow;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = slow;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = fast;
motor[TIRE_BL].dir = BACK;
mtc=true;
break;
case 253:
motor[TIRE_FR].pwm = slow;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = 0;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BR].pwm = 0;
motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].pwm = slow;
motor[TIRE_BL].dir = BACK;
mtc=true;
break;
case 254:
motor[TIRE_FR].pwm = normal;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = 0;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BR].pwm = 0;
motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].pwm = normal;
motor[TIRE_BL].dir = BACK;
mtc=true;
break;
case 1:
motor[TIRE_FR].pwm = slow;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = fast;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = fast;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = slow;
motor[TIRE_BL].dir = BACK;
mtc=true;
break;
case 3:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = slow;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = slow;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
mtc=true;
break;
case 2:
motor[TIRE_FR].pwm = 0;
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].pwm = normal;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = normal;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = 0;
motor[TIRE_BL].dir = FREE;
mtc=true;
break;
}
if(g[1]!=98&&mtc==true) {
switch(g[0]) {
case 0:
mtc=false;
break;
case 255:
motor[TIRE_FL].pwm += 3;
mtc=false;
break;
case 253:
motor[TIRE_FL].pwm += 6;
mtc=false;
break;
case 254:
motor[TIRE_FL].pwm += 9;
motor[TIRE_FR].pwm = 0;
mtc=false;
break;
case 1:
motor[TIRE_FR].pwm += 3;
mtc=false;
break;
case 3:
motor[TIRE_FR].pwm += 6;
mtc=false;
break;
case 2:
motor[TIRE_FR].pwm += 9;
motor[TIRE_FL].pwm = 0;
mtc=false;
break;
}
}
}
else if(mode==100) {
motor[TIRE_FR].pwm = 50;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].pwm = 50;
motor[TIRE_FL].dir = BACK;
motor[TIRE_BR].pwm = 50;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].pwm = 50;
motor[TIRE_BL].dir = BACK;
}
}
#endif
#if USE_PROCESS_NUM>4
static void Process4()
{
}
#endif
#if USE_PROCESS_NUM>5
static void Process5()
{
}
#endif
#if USE_PROCESS_NUM>6
static void Process6()
{
}
#endif
#if USE_PROCESS_NUM>7
static void Process7()
{
}
#endif
#if USE_PROCESS_NUM>8
static void Process8()
{
}
#endif
#if USE_PROCESS_NUM>9
static void Process9()
{
}
#endif
#endif
#pragma endregion PROCESS
static void AllActuatorReset()
{
#ifdef USE_SOLENOID
solenoid.all = ALL_SOLENOID_OFF;
#endif
#ifdef USE_MOTOR
for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++) {
motor[i].dir = FREE;
motor[i].pwm = 0;
}
#endif
}
#pragma region USER-DEFINED-FUNCTIONS
uint8_t SetStatus(int pwmVal)
{
if (pwmVal < 0) return BACK;
else if (pwmVal > 0) return FOR;
else if (pwmVal == 0) return BRAKE;
else return BRAKE;
}
uint8_t SetPWM(int pwmVal)
{
if (pwmVal == 0 || pwmVal > 255 || pwmVal < -255) return 255;
else return abs(pwmVal);
}
#pragma endregion