void PID_PosCfg(int16_t currPos, int16_t setPos, bool isLeft, PID_Config *config) {
int32_t pid, speed;
MOT_Direction direction=MOT_DIR_FORWARD;
pid = PID(currPos, setPos, config);
/* transform into motor speed */
speed = ((int32_t)config->maxSpeedPercent)*(0xffff/100); /* limit the speed */
pid = Limit(pid, -speed, speed);
if (pid<0) { /* move forward */
speed -= pid;
direction = MOT_DIR_FORWARD;
} else { /* move backward */
speed += pid;
direction = MOT_DIR_BACKWARD;
}
/* speed is now always positive, make sure it is within 16bit PWM boundary */
if (speed>0xFFFF) {
speed = 0xFFFF;
} else if (speed<0) {
speed = 0;
}
/* send new speed values to motor */
if (isLeft) {
MOT_SetVal(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0xFFFF-speed); /* PWM is low active */
MOT_SetDirection(MOT_GetMotorHandle(MOT_MOTOR_LEFT), direction);
} else {
MOT_SetVal(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0xFFFF-speed); /* PWM is low active */
MOT_SetDirection(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), direction);
}
}
void PID_LineCfg(uint16_t currLine, uint16_t setLine, PID_Config *config) {
int32_t pid, speed, speedL, speedR;
MOT_Direction directionL=MOT_DIR_FORWARD, directionR=MOT_DIR_FORWARD;
pid = PID(currLine, setLine, config);
/*! \todo Apply PID values to speed values */
speed = ((int32_t)config->maxSpeedPercent)*(0xffff/100); /* 100% */
pid = Limit(pid, -speed, speed);
if (pid<0) { /* turn right */
speedR = speed+pid;
speedL = speed-pid;
} else { /* turn left */
speedR = speed+pid;
speedL = speed-pid;
}
/* speed is now always positive, make sure it is within 16bit PWM boundary */
if (speedL>0xFFFF) {
speedL = 0xFFFF;
} else if (speedL<0) {
speedL = 0;
}
if (speedR>0xFFFF) {
speedR = 0xFFFF;
} else if (speedR<0) {
speedR = 0;
}
/* send new speed values to motor */
MOT_SetVal(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0xFFFF-speedL); /* PWM is low active */
MOT_SetDirection(MOT_GetMotorHandle(MOT_MOTOR_LEFT), directionL);
MOT_SetVal(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0xFFFF-speedR); /* PWM is low active */
MOT_SetDirection(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), directionR);
}
uint8_t MOT_ParseCommand(const unsigned char *cmd, bool *handled, const CLS1_StdIOType *io) {
uint8_t res = ERR_OK;
int32_t val;
const unsigned char *p;
if (UTIL1_strcmp((char*)cmd, (char*)CLS1_CMD_HELP)==0 || UTIL1_strcmp((char*)cmd, (char*)"motor help")==0) {
MOT_PrintHelp(io);
*handled = TRUE;
} else if (UTIL1_strcmp((char*)cmd, (char*)CLS1_CMD_STATUS)==0 || UTIL1_strcmp((char*)cmd, (char*)"motor status")==0) {
MOT_PrintStatus(io);
*handled = TRUE;
} else if (UTIL1_strcmp((char*)cmd, (char*)"motor L forward")==0) {
MOT_SetDirection(&motorL, MOT_DIR_FORWARD);
*handled = TRUE;
} else if (UTIL1_strcmp((char*)cmd, (char*)"motor R forward")==0) {
MOT_SetDirection(&motorR, MOT_DIR_FORWARD);
*handled = TRUE;
} else if (UTIL1_strcmp((char*)cmd, (char*)"motor both go")==0) {
MOT_StartMotors();
*handled = TRUE;
} else if (UTIL1_strcmp((char*)cmd, (char*)"motor both fw")==0) {
MOT_DriveForward();
*handled = TRUE;
} else if (UTIL1_strcmp((char*)cmd, (char*)"motor both bw")==0) {
MOT_DriveBackward();
*handled = TRUE;
} else if (UTIL1_strcmp((char*)cmd, (char*)"motor stop")==0) {
MOT_StopMotors();
*handled = TRUE;
} else if (UTIL1_strcmp((char*)cmd, (char*)"motor L backward")==0) {
MOT_SetDirection(&motorL, MOT_DIR_BACKWARD);
*handled = TRUE;
} else if (UTIL1_strcmp((char*)cmd, (char*)"motor R backward")==0) {
MOT_SetDirection(&motorR, MOT_DIR_BACKWARD);
*handled = TRUE;
} else if (UTIL1_strncmp((char*)cmd, (char*)"motor L duty ", sizeof("motor L duty ")-1)==0) {
p = cmd+sizeof("motor L duty");
if (UTIL1_xatoi(&p, &val)==ERR_OK && val >=-100 && val<=100) {
MOT_SetSpeedPercent(&motorL, (MOT_SpeedPercent)val);
*handled = TRUE;
} else {
CLS1_SendStr((unsigned char*)"Wrong argument, must be in the range -100..100\r\n", io->stdErr);
res = ERR_FAILED;
}
} else if (UTIL1_strncmp((char*)cmd, (char*)"motor R duty ", sizeof("motor R duty ")-1)==0) {
p = cmd+sizeof("motor R duty");
if (UTIL1_xatoi(&p, &val)==ERR_OK && val >=-100 && val<=100) {
MOT_SetSpeedPercent(&motorR, (MOT_SpeedPercent)val);
*handled = TRUE;
} else {
CLS1_SendStr((unsigned char*)"Wrong argument, must be in the range -100..100\r\n", io->stdErr);
res = ERR_FAILED;
}
}
return res;
}
void PID_PosCfg(int32_t currPos, int32_t setPos, bool isLeft, PID_Config *config) {
int32_t pwm;
MOT_Direction direction=MOT_DIR_FORWARD;
MOT_MotorDevice *motHandle;
pwm = PID(currPos, setPos, config);
/* transform into motor pwm */
pwm *= 1000; /* scale PID, otherwise we need high PID constants */
if (pwm>=0) {
direction = MOT_DIR_FORWARD;
} else { /* negative, make it positive */
pwm = -pwm; /* make positive */
direction = MOT_DIR_BACKWARD;
}
/* pwm is now always positive, make sure it is within 16bit PWM boundary */
if (pwm>0xFFFF) {
pwm = 0xFFFF;
}
/* send new pwm values to motor */
if (isLeft) {
motHandle = MOT_GetMotorHandle(MOT_MOTOR_LEFT);
} else {
motHandle = MOT_GetMotorHandle(MOT_MOTOR_RIGHT);
}
MOT_SetVal(motHandle, 0xFFFF-pwm); /* PWM is low active */
MOT_SetDirection(motHandle, direction);
MOT_UpdatePercent(motHandle, direction);
}
void PID_SpeedCfg(int32_t currSpeed, int32_t setSpeed, bool isLeft, PID_Config *config) {
int32_t speed;
MOT_Direction direction=MOT_DIR_FORWARD;
MOT_MotorDevice *motHandle;
speed = PID(currSpeed, setSpeed, config);
if (speed>=0) {
direction = MOT_DIR_FORWARD;
} else { /* negative, make it positive */
speed = -speed; /* make positive */
direction = MOT_DIR_BACKWARD;
}
/* speed shall be positive here, make sure it is within 16bit PWM boundary */
if (speed>0xFFFF) {
speed = 0xFFFF;
}
/* send new speed values to motor */
if (isLeft) {
motHandle = MOT_GetMotorHandle(MOT_MOTOR_LEFT);
} else {
motHandle = MOT_GetMotorHandle(MOT_MOTOR_RIGHT);
}
MOT_SetVal(motHandle, 0xFFFF-speed); /* PWM is low active */
MOT_SetDirection(motHandle, direction);
MOT_UpdatePercent(motHandle, direction);
}
void MOT_SetSpeedPercent(MOT_MotorDevice *motor, MOT_SpeedPercent percent) {
uint32_t val;
motor->currSpeedPercent = percent; /* store current value */
if (percent<0) {
MOT_SetDirection(motor, MOT_DIR_BACKWARD);
percent = -percent;
} else {
MOT_SetDirection(motor, MOT_DIR_FORWARD);
}
if (percent>100) { /* make sure we are within 0..100 */
percent = 100;
}
val = ((100-percent)*0xffff)/100;
MOT_SetVal(motor, (uint16_t)val);
}
void MOT_SetSpeedPercent(MOT_MotorDevice *motor, MOT_SpeedPercent percent) {
/*! \todo See lab guide about this function */
uint32_t val;
if (percent>100) { /* make sure we are within 0..100 */
percent = 100;
} else if (percent<-100) {
percent = -100;
}
motor->currSpeedPercent = percent; /* store value */
if (percent<0) {
MOT_SetDirection(motor, MOT_DIR_BACKWARD);
percent = -percent; /* make it positive */
} else {
MOT_SetDirection(motor, MOT_DIR_FORWARD);
}
val = ((100-percent)*0xffff)/100; /* H-Bridge is low active! */
MOT_SetVal(motor, (uint16_t)val);
}
/*
* \brief -100% bis 100% wobei Minuswerte Rückwärts drehen...
* \author Philippe Schenker
* \todo Performance verbessern
*/
void MOT_SetSpeedPercent(MOT_MotorSide side, MOT_SpeedPercent percent) {
uint16_t newRatio = 0;
MOT_MotorDevice* motor = MOT_GetMotorHandle(side);
if(side==MOT_MOTOR_RIGHT) {
percent *= -1;
}
if(percent >= 0) { /* FORWARD */
if(percent > 100) percent = 100;
percent = 100 - percent;
newRatio = (int) (655.35*percent);
MOT_SetDirection(motor, MOT_DIR_FORWARD);
} else if(percent < 0) { /* BACKWARD */
percent = abs(percent);
if(percent > 100) percent = 100;
percent = 100 - percent;
newRatio = (int) (655.35*percent);
MOT_SetDirection(motor, MOT_DIR_BACKWARD);
}
MOT_SetVal(motor, newRatio);
}
void PID_PosCfg(int32_t currPos, int32_t setPos, bool isLeft, PID_Config *config) {
int32_t speed, val;
MOT_Direction direction=MOT_DIR_FORWARD;
MOT_MotorDevice *motHandle;
int error;
#define POS_FILTER 5
error = setPos-currPos;
if (error>-POS_FILTER && error<POS_FILTER) { /* avoid jitter around zero */
setPos = currPos;
}
speed = PID(currPos, setPos, config);
/* transform into motor speed */
speed *= 1000; /* scale PID, otherwise we need high PID constants */
if (speed>=0) {
direction = MOT_DIR_FORWARD;
} else { /* negative, make it positive */
speed = -speed; /* make positive */
direction = MOT_DIR_BACKWARD;
}
/* speed is now always positive, make sure it is within 16bit PWM boundary */
if (speed>0xFFFF) {
speed = 0xFFFF;
}
#if 1
/* limit speed to maximum value */
val = ((int32_t)config->maxSpeedPercent)*(0xffff/100); /* 100% */
speed = Limit(speed, -val, val);
#else
/* limit speed to maximum value */
speed = (speed*config->maxSpeedPercent)/100;
#endif
/* send new speed values to motor */
if (isLeft) {
motHandle = MOT_GetMotorHandle(MOT_MOTOR_LEFT);
} else {
motHandle = MOT_GetMotorHandle(MOT_MOTOR_RIGHT);
}
MOT_SetVal(motHandle, 0xFFFF-speed); /* PWM is low active */
MOT_SetDirection(motHandle, direction);
MOT_UpdatePercent(motHandle, direction);
}
void PID_LineCfg(uint16_t currLine, uint16_t setLine, PID_Config *config) {
int32_t pid, speed, speedL, speedR;
MOT_Direction directionL=MOT_DIR_FORWARD, directionR=MOT_DIR_FORWARD;
pid = PID(currLine, setLine, config);
uint8_t errorPercent;
errorPercent = errorWithinPercent(currLine-setLine);
/* change speed depending on error */
/* transform into different speed for motors. The PID is used as difference value to the motor PWM */
if (errorPercent <= 20) { /* pretty on center: move forward both motors with base speed */
speed = ((int32_t)config->maxSpeedPercent)*(0xffff/100); /* 100% */
pid = Limit(pid, -speed, speed);
if (pid<0) { /* turn right */
speedR = speed;
speedL = speed-pid;
} else { /* turn left */
speedR = speed+pid;
speedL = speed;
}
} else if (errorPercent <= 40) {
/* outside left/right halve position from center, slow down one motor and speed up the other */
speed = ((int32_t)config->maxSpeedPercent)*(0xffff/100)*8/10; /* 80% */
pid = Limit(pid, -speed, speed);
if (pid<0) { /* turn right */
speedR = speed+pid; /* decrease speed */
speedL = speed-pid; /* increase speed */
} else { /* turn left */
speedR = speed+pid; /* increase speed */
speedL = speed-pid; /* decrease speed */
}
} else if (errorPercent <= 70) {
speed = ((int32_t)config->maxSpeedPercent)*(0xffff/100)*6/10; /* %60 */
pid = Limit(pid, -speed, speed);
if (pid<0) { /* turn right */
speedR = 0 /*maxSpeed+pid*/; /* decrease speed */
speedL = speed-pid; /* increase speed */
} else { /* turn left */
speedR = speed+pid; /* increase speed */
speedL = 0 /*maxSpeed-pid*/; /* decrease speed */
}
} else {
/* line is far to the left or right: use backward motor motion */
speed = ((int32_t)config->maxSpeedPercent)*(0xffff/100)*10/10; /* %80 */
if (pid<0) { /* turn right */
speedR = -speed+pid; /* decrease speed */
speedL = speed-pid; /* increase speed */
} else { /* turn left */
speedR = speed+pid; /* increase speed */
speedL = -speed-pid; /* decrease speed */
}
speedL = Limit(speedL, -speed, speed);
speedR = Limit(speedR, -speed, speed);
directionL = AbsSpeed(&speedL);
directionR = AbsSpeed(&speedR);
}
/* speed is now always positive, make sure it is within 16bit PWM boundary */
if (speedL>0xFFFF) {
speedL = 0xFFFF;
} else if (speedL<0) {
speedL = 0;
}
if (speedR>0xFFFF) {
speedR = 0xFFFF;
} else if (speedR<0) {
speedR = 0;
}
/* send new speed values to motor */
MOT_SetVal(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0xFFFF-speedL); /* PWM is low active */
MOT_SetDirection(MOT_GetMotorHandle(MOT_MOTOR_LEFT), directionL);
MOT_SetVal(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0xFFFF-speedR); /* PWM is low active */
MOT_SetDirection(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), directionR);
}
void MOT_DriveBackward(){
MOT_SetDirection(&motorL, MOT_DIR_BACKWARD);
MOT_SetDirection(&motorR, MOT_DIR_BACKWARD);
}
void MOT_DriveForward(){
MOT_SetDirection(&motorL, MOT_DIR_FORWARD);
MOT_SetDirection(&motorR, MOT_DIR_FORWARD);
}
void PID_LineCfg(uint16_t currLine, uint16_t setLine, PID_Config *config) {
int32_t pid, speed, speedL, speedR;
#if PID_DEBUG
unsigned char buf[16];
static uint8_t cnt = 0;
#endif
uint8_t errorPercent;
MOT_Direction directionL=MOT_DIR_FORWARD, directionR=MOT_DIR_FORWARD;
pid = PID(currLine, setLine, config);
errorPercent = errorWithinPercent(currLine-setLine);
/* transform into different speed for motors. The PID is used as difference value to the motor PWM */
if (errorPercent <= 20) { /* pretty on center: move forward both motors with base speed */
speed = ((int32_t)config->maxSpeedPercent)*(0xffff/100); /* 100% */
pid = Limit(pid, -speed, speed);
if (pid<0) { /* turn right */
speedR = speed;
speedL = speed-pid;
} else { /* turn left */
speedR = speed+pid;
speedL = speed;
}
} else if (errorPercent <= 40) {
/* outside left/right halve position from center, slow down one motor and speed up the other */
speed = ((int32_t)config->maxSpeedPercent)*(0xffff/100)*8/10; /* 80% */
pid = Limit(pid, -speed, speed);
if (pid<0) { /* turn right */
speedR = speed+pid; /* decrease speed */
speedL = speed-pid; /* increase speed */
} else { /* turn left */
speedR = speed+pid; /* increase speed */
speedL = speed-pid; /* decrease speed */
}
} else if (errorPercent <= 70) {
speed = ((int32_t)config->maxSpeedPercent)*(0xffff/100)*6/10; /* %60 */
pid = Limit(pid, -speed, speed);
if (pid<0) { /* turn right */
speedR = 0 /*maxSpeed+pid*/; /* decrease speed */
speedL = speed-pid; /* increase speed */
} else { /* turn left */
speedR = speed+pid; /* increase speed */
speedL = 0 /*maxSpeed-pid*/; /* decrease speed */
}
} else {
/* line is far to the left or right: use backward motor motion */
speed = ((int32_t)config->maxSpeedPercent)*(0xffff/100)*10/10; /* %80 */
if (pid<0) { /* turn right */
speedR = -speed+pid; /* decrease speed */
speedL = speed-pid; /* increase speed */
} else { /* turn left */
speedR = speed+pid; /* increase speed */
speedL = -speed-pid; /* decrease speed */
}
speedL = Limit(speedL, -speed, speed);
speedR = Limit(speedR, -speed, speed);
directionL = AbsSpeed(&speedL);
directionR = AbsSpeed(&speedR);
}
/* speed is now always positive, make sure it is within 16bit PWM boundary */
if (speedL>0xFFFF) {
speedL = 0xFFFF;
} else if (speedL<0) {
speedL = 0;
}
if (speedR>0xFFFF) {
speedR = 0xFFFF;
} else if (speedR<0) {
speedR = 0;
}
/* send new speed values to motor */
MOT_SetVal(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0xFFFF-speedL); /* PWM is low active */
MOT_SetDirection(MOT_GetMotorHandle(MOT_MOTOR_LEFT), directionL);
MOT_SetVal(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0xFFFF-speedR); /* PWM is low active */
MOT_SetDirection(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), directionR);
#if PID_DEBUG /* debug diagnostic */
{
cnt++;
if (cnt>10) { /* limit number of messages to the console */
CLS1_StdIO_OutErr_FctType ioOut = CLS1_GetStdio()->stdOut;
cnt = 0;
CLS1_SendStr((unsigned char*)"line:", ioOut);
buf[0] = '\0';
UTIL1_strcatNum16u(buf, sizeof(buf), currLine);
CLS1_SendStr(buf, ioOut);
CLS1_SendStr((unsigned char*)" sum:", ioOut);
buf[0] = '\0';
UTIL1_strcatNum32Hex(buf, sizeof(buf), integral);
CLS1_SendStr(buf, ioOut);
CLS1_SendStr((unsigned char*)" left:", ioOut);
CLS1_SendStr(directionL==MOT_DIR_FORWARD?(unsigned char*)"fw ":(unsigned char*)"bw ", ioOut);
buf[0] = '\0';
UTIL1_strcatNum16Hex(buf, sizeof(buf), speedL);
CLS1_SendStr(buf, ioOut);
CLS1_SendStr((unsigned char*)" right:", ioOut);
CLS1_SendStr(directionR==MOT_DIR_FORWARD?(unsigned char*)"fw ":(unsigned char*)"bw ", ioOut);
buf[0] = '\0';
UTIL1_strcatNum16Hex(buf, sizeof(buf), speedR);
CLS1_SendStr(buf, ioOut);
CLS1_SendStr((unsigned char*)"\r\n", ioOut);
}
}
#endif
}
uint8_t MOT_ParseCommand(const unsigned char *cmd, bool *handled, const CLS1_StdIOType *io) {
uint8_t res = ERR_OK;
long val;
const unsigned char *p;
if (UTIL1_strcmp((char*)cmd, (char*)CLS1_CMD_HELP)==0 || UTIL1_strcmp((char*)cmd, (char*)"motor help")==0) {
MOT_PrintHelp(io);
*handled = TRUE;
} else if (UTIL1_strcmp((char*)cmd, (char*)CLS1_CMD_STATUS)==0 || UTIL1_strcmp((char*)cmd, (char*)"motor status")==0) {
MOT_PrintStatus(io);
*handled = TRUE;
} else if (UTIL1_strcmp((char*)cmd, (char*)"motor L forward")==0) {
MOT_SetDirection(&motorL, MOT_DIR_FORWARD);
*handled = TRUE;
} else if (UTIL1_strcmp((char*)cmd, (char*)"motor R forward")==0) {
MOT_SetDirection(&motorR, MOT_DIR_FORWARD);
*handled = TRUE;
} else if (UTIL1_strcmp((char*)cmd, (char*)"motor L backward")==0) {
MOT_SetDirection(&motorL, MOT_DIR_BACKWARD);
*handled = TRUE;
} else if (UTIL1_strcmp((char*)cmd, (char*)"motor R backward")==0) {
MOT_SetDirection(&motorR, MOT_DIR_BACKWARD);
*handled = TRUE;
} else if (UTIL1_strncmp((char*)cmd, (char*)"motor L duty ", sizeof("motor L duty ")-1)==0) {
if (!isMotorOn) {
CLS1_SendStr((unsigned char*)"Motor is OFF, cannot set duty.\r\n", io->stdErr);
res = ERR_FAILED;
} else {
p = cmd+sizeof("motor L duty");
if (UTIL1_xatoi(&p, &val)==ERR_OK && val >=-100 && val<=100) {
MOT_SetSpeedPercent(&motorL, (MOT_SpeedPercent)val);
*handled = TRUE;
} else {
CLS1_SendStr((unsigned char*)"Wrong argument, must be in the range -100..100\r\n", io->stdErr);
res = ERR_FAILED;
}
}
} else if (UTIL1_strncmp((char*)cmd, (char*)"motor R duty ", sizeof("motor R duty ")-1)==0) {
if (!isMotorOn) {
CLS1_SendStr((unsigned char*)"Motor is OFF, cannot set duty.\r\n", io->stdErr);
res = ERR_FAILED;
} else {
p = cmd+sizeof("motor R duty");
if (UTIL1_xatoi(&p, &val)==ERR_OK && val >=-100 && val<=100) {
MOT_SetSpeedPercent(&motorR, (MOT_SpeedPercent)val);
*handled = TRUE;
} else {
CLS1_SendStr((unsigned char*)"Wrong argument, must be in the range -100..100\r\n", io->stdErr);
res = ERR_FAILED;
}
}
} else if (UTIL1_strncmp((char*)cmd, (char*)"motor on", sizeof("motor on")-1)==0) {
isMotorOn = TRUE;
*handled = TRUE;
} else if (UTIL1_strncmp((char*)cmd, (char*)"motor off", sizeof("motor off")-1)==0) {
MOT_SetSpeedPercent(&motorL, 0);
MOT_SetSpeedPercent(&motorR, 0);
isMotorOn = FALSE;
*handled = TRUE;
#if PL_HAS_MOTOR_BRAKE
} else if (UTIL1_strcmp((char*)cmd, (char*)"motor L brake on")==0) {
MOT_SetBrake(&motorL, TRUE);
*handled = TRUE;
} else if (UTIL1_strcmp((char*)cmd, (char*)"motor L brake off")==0) {
MOT_SetBrake(&motorL, FALSE);
*handled = TRUE;
} else if (UTIL1_strcmp((char*)cmd, (char*)"motor R brake on")==0) {
MOT_SetBrake(&motorR, TRUE);
*handled = TRUE;
} else if (UTIL1_strcmp((char*)cmd, (char*)"motor R brake off")==0) {
MOT_SetBrake(&motorR, FALSE);
*handled = TRUE;
#endif
}
return res;
}
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 */
}
