static void StateMachine_Run(void){
int i;
int distance;
switch (state) {
case START:
WAIT1_WaitOSms(800);
BUZ_Beep(400,400);
WAIT1_WaitOSms(800);
BUZ_Beep(450,400);
WAIT1_WaitOSms(800);
BUZ_Beep(500,400);
WAIT1_WaitOSms(800);
BUZ_Beep(600,400);
WAIT1_WaitOSms(800);
BUZ_Beep(880,400);
MOT_SetDirection(MOT_GetMotorHandle(MOT_MOTOR_LEFT), MOT_DIR_FORWARD);
MOT_SetDirection(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), MOT_DIR_FORWARD);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 40);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 40);
/*int x0;
int x5;
int x0_cal;
int x5_cal;
x0 = Get_Reflectance_Values(0);
x0_cal = x0 / 15;
x5 = Get_Reflectance_Values(5);
x5_cal = x5 / 15;
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), x0_cal);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), x5_cal);
*/
//uint16_t* calib_values_pointer;
//calib_values_pointer = &SensorTimeType;
//calib_values_pointer = S1_GetVal();
state = DRIVE;
break;
case DRIVE:
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), SPEED_TURN);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), -SPEED_TURN);
if((Get_Reflectance_Values(3) <= 100) || (Get_Reflectance_Values(4) <= 100)){ // 0 = weiss / 1000 = schwarz
state = STOP;
}
distance = US_usToCentimeters(US_Measure_us(),22);
if((distance <= 30) && (distance != 0)){
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), SPEED_ATTACK);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), SPEED_ATTACK);
state = ATTACK;
}
/*if((MMA1_GetXmg() >= 500) || (MMA1_GetXmg() <= -500)){
state = STOP;
}
*/
/*if((MMA1_GetYmg() >= 500) || (MMA1_GetYmg() <= -500)){
state = STOP;
}*/
break;
case TURN:
state = DRIVE;
break;
case STOP:
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), -50);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), -50);
WAIT1_WaitOSms(150);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), -50);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 50);
WAIT1_WaitOSms(444);
if(Get_Reflectance_Values(4) >= 400){
state = DRIVE;
}
/*if((MMA1_GetXmg() >= -200) && (MMA1_GetXmg() <= 200)){
state = DRIVE;
}*/
/*if((MMA1_GetYmg() >= -200) || (MMA1_GetYmg() <= 200)){
state = DRIVE;
}*/
break;
case ATTACK:
if((distance <= 30) && (distance != 0)){
state = ATTACK;
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), SPEED_ATTACK);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), SPEED_ATTACK);
WAIT1_WaitOSms(50);
}else{
state = DRIVE;
}
if((Get_Reflectance_Values(3) <= 100) || (Get_Reflectance_Values(4) <= 100)){ // 0 = weiss / 1000 = schwarz
state = STOP;
}
break;
} /* switch */
}
static void StateMachine_Run(void){
int i;
int distance;
// Für Acceleration Sensor
int16_t x = MMA1_GetXmg();
WAIT1_Waitms(5);
int16_t y = MMA1_GetYmg();
switch (state) {
case START:
//SHELL_SendString((unsigned char*)"INFO: No calibration data present.\r\n");
WAIT1_Waitms(4700);
DRV_EnableDisable(1);
DRV_SetSpeed(3000,3000);
//WAIT1_Waitms(40);
//DRV_SetSpeed(3000,3000);
//state = ATTACK;
//DRV_SetSpeed(-4200,4200);
// MOT_SetDirection(MOT_GetMotorHandle(MOT_MOTOR_LEFT), MOT_DIR_FORWARD);
//MOT_SetDirection(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), MOT_DIR_FORWARD);
//MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 30);
//MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 30);
//MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 40);
/*int x0;
int x5;
int x0_cal;
int x5_cal;
x0 = Get_Reflectance_Values(0);
x0_cal = x0 / 15;
x5 = Get_Reflectance_Values(5);
x5_cal = x5 / 15;
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), x0_cal);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), x5_cal);
*/
//uint16_t* calib_values_pointer;
//calib_values_pointer = &SensorTimeType;
//calib_values_pointer = S1_GetVal();
state = DRIVE;
break;
case DRIVE:
DRV_SetSpeed(-4200,4200);
//MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 40);
//MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 40);
if((Get_Reflectance_Values(0) <= 100) || (Get_Reflectance_Values(5) <= 100)) // 0 = weiss / 1000 = schwarz
{
state = TURN;
break;
}
distance = US_usToCentimeters(US_Measure_us(),22);
if((distance <= 30) && (distance != 0)){
state = ATTACK;
}
/*
if((x >= 2000) || (x <= 2000))
{
state = STOP;
}
if((y >= 2000) || (y <= 2000))
{
state = STOP;
}
*/
break;
case TURN:
//SHELL_SendString((unsigned char*)"...stopping calibration.\r\n");
//MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), -40);
//MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), -40);
// ATTACK
DRV_SetSpeed(-5000,-5000);
WAIT1_Waitms(500);
/*
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), -40);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 40);
*/
DRV_SetSpeed(4200,-4200);
WAIT1_Waitms(300);
DRV_SetSpeed(5000,5000);
WAIT1_Waitms(50);
if((Get_Reflectance_Values(0) > 400) || (Get_Reflectance_Values(5) > 500))
{
state = DRIVE;
}
break;
case ATTACK:
DRV_SetSpeed(5500,5500);
//MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 80);
//MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 80);
if((Get_Reflectance_Values(0) <= 100) || (Get_Reflectance_Values(5) <= 100)) // 0 = weiss / 1000 = schwarz
{
state = TURN;
}
/*
if((x >= 2000) || (x <= -2000))
{
state = STOP;
}
if((y>= 2000) || (y <= -2000))
{
state = STOP;
}
*/
break;
case STOP:
DRV_SetSpeed(0,0);
/*MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);*/
/*
if((x >= -300) && (x <= 300))
{
if((y >= -300) && (y <= 300))
{
state = DRIVE;
}
}
*/
break;
} /* whitch */
}
