static void FollowObstacle(void) {
#define DUTY_SLOW 16
#define DUTY_MEDIUM 20
#define DUTY_FAST 23
static uint8_t cnt;
uint16_t cm, us;
cnt++; /* get called with 100 Hz, reduce to 10 Hz */
if (cnt==10) {
us = US_Measure_us();
cnt = 0;
if (followObstacle) {
cm = US_usToCentimeters(us, 22);
if (cm<10) { /* back up! */
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), -DUTY_SLOW);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), -DUTY_SLOW);
} else if (cm>=10 && cm<=20) { /* stand still */
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
TURN_Turn(TURN_RIGHT45); /* try to avoid obstacle */
} else if (cm>20 && cm<=40) { /* forward slowly */
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), DUTY_MEDIUM);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), DUTY_MEDIUM);
} else if (cm>40 && cm<100) { /* forward fast */
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), DUTY_FAST);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), DUTY_FAST);
} else { /* nothing in range */
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
}
}
}
}
void Measure(void) {
int us, cm;
int buf[8];
us = US_Measure_us();
cm = US_usToCentimeters(us, 22);
if(cm < 10){
//LED_SetVal();
}
}
static void US_PrintStatus(const CLS1_StdIOType *io) {
uint8_t buf[16];
uint16_t cm;
cm = US_usToCentimeters(usDevice.lastValue_us, 22);
CLS1_SendStatusStr((unsigned char*)"ultrasonic", (unsigned char*)"\r\n", io->stdOut);
UTIL1_Num16uToStr(buf, sizeof(buf), usDevice.lastValue_us);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
CLS1_SendStatusStr((unsigned char*)" us", buf, io->stdOut);
UTIL1_Num16uToStr(buf, sizeof(buf), cm);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
CLS1_SendStatusStr((unsigned char*)" cm", buf, io->stdOut);
}
static uint8_t MeasureCm(void) {
uint16_t us, cm;
uint8_t buf[8];
us = US_Measure_us();
UTIL1_Num16uToStrFormatted(buf, sizeof(buf), us, ' ', 5);
//LCD1_GotoXY(1,5);
//LCD1_WriteString((char*)buf);
cm = US_usToCentimeters(us, 22);
UTIL1_Num16uToStrFormatted(buf, sizeof(buf), cm, ' ', 5);
//LCD1_GotoXY(2,5);
//LCD1_WriteString((char*)buf);
LEDR_Put(cm>0 && cm<10); /* red LED if object closer than 10 cm */
// LEDB_Put(cm>=10&&cm<=100); /* blue LED if object is in 10..100 cm range */
LEDG_Put(cm>=10); /* blue LED if object is in 10..100 cm range */
return cm;
}
uint16_t US_GetLastCentimeterValue(void) {
return US_usToCentimeters(usDevice.lastValue_us, 22);
}
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 */
}