void REF_Danger(void){
int i = 0;
uint8_t sensor = 0;
bool temp;
if(!EVNT_EventIsSet(EVNT_DONT_FALL_DOWN))
for( i=0;i<REF_NOF_SENSORS;i++) {
if(SensorCalibrated[i] < 1000-THRESHOLD_BLK && SensorCalibrated[i]!=0 ){
sensor = i;
if(fightOn){
fight_state = WHITE_LINE_DETECTION;
FRTOS1_vTaskSuspend(checkRefl);
}
if(remoteOn){
EVNT_SetEvent(EVNT_REMOTE_DEACTIVATE);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), -30);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), -30);
vTaskDelay(500/TRG_TICKS_MS);
EVNT_SetEvent(EVNT_REMOTE_ACTIVATE);
}
else {
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT) , 0);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
EVNT_SetEvent(EVNT_DONT_FALL_DOWN);
FRTOS1_vTaskSuspend(checkRefl);
}
}
}
}
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 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);
}
static portTASK_FUNCTION(DriveTask, pvParameters) {
(void)pvParameters; /* parameter not used */
bool prevOn;
//traceLabel usrEvent;
prevOn = DRV_SpeedOn;
#if PL_HAS_RTOS_TRACE
//usrEvent = xTraceOpenLabel("drive");
#endif
for(;;) {
#if PL_HAS_RTOS_TRACE
//RTOSTRC1_vTraceUserEvent(usrEvent);
#endif
/*! \todo extend this for your own needs and with a position PID */
TACHO_CalcSpeed();
if (prevOn && !DRV_SpeedOn) { /* turned off */
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
PID_Start(); /* reset values */
} else if (DRV_SpeedOn) {
PID_Speed(TACHO_GetSpeed(TRUE), DRV_SpeedLeft, TRUE); /* left */
PID_Speed(TACHO_GetSpeed(FALSE), DRV_SpeedRight, FALSE); /* right */
}
prevOn = DRV_SpeedOn;
FRTOS1_vTaskDelay(2/portTICK_RATE_MS);
} /* for */
}
uint8_t REMOTE_ParseCommand(const unsigned char *cmd, bool *handled, const CLS1_StdIOType *io) {
uint8_t res = ERR_OK;
if (UTIL1_strcmp((char*)cmd, (char*)CLS1_CMD_HELP)==0 || UTIL1_strcmp((char*)cmd, (char*)"remote help")==0) {
REMOTE_PrintHelp(io);
*handled = TRUE;
} else if (UTIL1_strcmp((char*)cmd, (char*)CLS1_CMD_STATUS)==0 || UTIL1_strcmp((char*)cmd, (char*)"remote status")==0) {
REMOTE_PrintStatus(io);
*handled = TRUE;
} else if (UTIL1_strcmp((char*)cmd, (char*)"remote on")==0) {
REMOTE_isOn = TRUE;
*handled = TRUE;
} else if (UTIL1_strcmp((char*)cmd, (char*)"remote off")==0) {
#if PL_CONFIG_HAS_MOTOR
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
#endif
REMOTE_isOn = FALSE;
*handled = TRUE;
} else if (UTIL1_strcmp((char*)cmd, (char*)"remote verbose on")==0) {
REMOTE_isVerbose = TRUE;
*handled = TRUE;
} else if (UTIL1_strcmp((char*)cmd, (char*)"remote verbose off")==0) {
REMOTE_isVerbose = FALSE;
*handled = TRUE;
} else if (UTIL1_strcmp((char*)cmd, (char*)"remote joystick on")==0) {
REMOTE_useJoystick = TRUE;
*handled = TRUE;
} else if (UTIL1_strcmp((char*)cmd, (char*)"remote joystick off")==0) {
REMOTE_useJoystick = FALSE;
*handled = TRUE;
}
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_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);
}
byte QUADCALIB_ParseCommand(const unsigned char *cmd, bool *handled, const CLS1_StdIOType *io) {
if (UTIL1_strcmp((char*)cmd, CLS1_CMD_HELP)==0 || UTIL1_strcmp((char*)cmd, "quadcalib help")==0) {
*handled = TRUE;
return PrintHelp(io);
} else if ((UTIL1_strcmp((char*)cmd, CLS1_CMD_STATUS)==0) || (UTIL1_strcmp((char*)cmd, "quadcalib status")==0)) {
*handled = TRUE;
return PrintStatus(io);
} else if ((UTIL1_strcmp((char*)cmd, "quadcalib tune 0")==0)) {
*handled = TRUE;
return Tune(io, 0, MOT_GetMotorHandle(MOT_MOTOR_RIGHT));
} else if ((UTIL1_strcmp((char*)cmd, "quadcalib tune 1")==0)) {
*handled = TRUE;
return Tune(io, 1, MOT_GetMotorHandle(MOT_MOTOR_RIGHT));
} else if ((UTIL1_strcmp((char*)cmd, "quadcalib tune 2")==0)) {
*handled = TRUE;
return Tune(io, 2, MOT_GetMotorHandle(MOT_MOTOR_LEFT));
} else if ((UTIL1_strcmp((char*)cmd, "quadcalib tune 3")==0)) {
*handled = TRUE;
return Tune(io, 3, MOT_GetMotorHandle(MOT_MOTOR_LEFT));
}
return ERR_OK;
}
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);
}
static void Turn(bool isLeft, uint8_t duty, uint16_t val) {
#if PL_HAS_QUADRATURE
(void)duty;
if (isLeft) {
TURN_TurnSteps(-val, val);
} else {
TURN_TurnSteps(val, -val);
}
#else
if (isLeft) {
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), -duty);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), duty);
} else {
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), duty);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), -duty);
}
WAIT1_WaitOSms(val); /* only use waiting time */
#endif
#if TURN_STOP_AFTER_TURN
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
#if TURN_WAIT_AFTER_STEP_MS > 0
WAIT1_WaitOSms(TURN_WAIT_AFTER_STEP_MS);
#endif
#endif
}
void MOT_SetDirection(MOT_MotorDevice *motor, MOT_Direction dir) {
/*
* \brief Direction anpassen für Phippus Karren
*/
#ifndef LOCAL_INVERT_MOT_DIR
if(motor == MOT_GetMotorHandle(MOT_MOTOR_RIGHT)) {
if(dir == MOT_DIR_FORWARD) {
dir = MOT_DIR_BACKWARD;
} else {
dir = MOT_DIR_FORWARD;
}
}
#endif
/*
* \brief Direction Anpassen für Bächlers Schlitten
*/
#ifdef LOCAL_INVERT_MOT_DIR
if(motor == MOT_GetMotorHandle(MOT_MOTOR_LEFT)) {
if(dir == MOT_DIR_FORWARD) {
dir = MOT_DIR_BACKWARD;
} else {
dir = MOT_DIR_FORWARD;
}
}
#endif
if (dir==MOT_DIR_BACKWARD) {
motor->DirPutVal(1);
if (motor->currSpeedPercent>0) {
motor->currSpeedPercent = -motor->currSpeedPercent;
}
} else if (dir==MOT_DIR_FORWARD) {
motor->DirPutVal(0);
if (motor->currSpeedPercent<0) {
motor->currSpeedPercent = -motor->currSpeedPercent;
}
}
}
/*
* \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);
}
static uint8_t Tune(const CLS1_StdIOType *io) {
uint8_t channel;
uint16_t dac;
int i;
QuadTime_t timing;
uint8_t buf[32];
uint8_t res;
uint16_t dacArr[4];
uint8_t pd[4];
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 10);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 10);
for(i=0;i<4;i++) { /* init */
dacArr[i] = 0;
pd[i] = 0;
}
CLS1_SendStr((uint8_t*)"Tuning....\r\n", io->stdOut);
res = ERR_OK;
for(channel=0;channel<NOF_SIGNALS;channel++) {
for(i=0,dac=0;i<80 && dac<MPC4728_MAX_DAC_VAL;i++,dac += 0x10) {
UTIL1_strcpy(buf, sizeof(buf), (uint8_t*)"Channel: ");
UTIL1_strcatNum8u(buf, sizeof(buf), channel);
UTIL1_strcat(buf, sizeof(buf), (uint8_t*)" DAC: 0x");
UTIL1_strcatNum16Hex(buf, sizeof(buf), dac);
UTIL1_chcat(buf, sizeof(buf), ' ');
CLS1_SendStr(buf, io->stdOut);
dacArr[channel] = dac;
if (MPC4728_FastWriteDAC(dacArr, sizeof(dacArr), pd, sizeof(pd))!=ERR_OK) {
CLS1_SendStr((uint8_t*)"ERROR writing DAC/EE\r\n", io->stdErr);
res = ERR_FAILED;
break;
}
WAIT1_Waitms(50); /* wait some time to allow change */
if (Measure(channel, &timing)==ERR_OK) {
buf[0] = '\0';
UTIL1_strcatNum8u(buf, sizeof(buf), timing.highPercent);
UTIL1_strcat(buf, sizeof(buf), (uint8_t*)"% high, low ");
UTIL1_strcatNum8u(buf, sizeof(buf), timing.lowPercent);
UTIL1_strcat(buf, sizeof(buf), (uint8_t*)"%\r\n");
CLS1_SendStr(buf, io->stdOut);
if (timing.highPercent==50 || timing.lowPercent==50) {
CLS1_SendStr((uint8_t*)"Set!\r\n", io->stdErr);
break;
}
} else {
CLS1_SendStr((uint8_t*)"No signal\r\n", io->stdErr);
}
} /* for */
if (res!=ERR_OK) {
CLS1_SendStr((uint8_t*)"ERROR!\r\n", io->stdErr);
break;
}
} /* for */
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
CLS1_SendStr((uint8_t*)"Writing to EEPROM...\r\n", io->stdOut);
for (channel=0;channel<NOF_SIGNALS;channel++) {
if (MPC4728_SingleWriteDACandEE(channel, dacArr[channel])!=ERR_OK) {
CLS1_SendStr((uint8_t*)"ERROR writing DAC/EE\r\n", io->stdErr);
res = ERR_FAILED;
break;
}
WAIT1_Waitms(500); /* give EEPROM time to write data */
}
CLS1_SendStr((uint8_t*)"finished!\r\n", io->stdOut);
return res;
}
static void REMOTE_HandleMotorMsg(int16_t speedVal, int16_t directionVal, int16_t z) {
#define SCALE_DOWN 30
#define MIN_VALUE 250 /* values below this value are ignored */
#define DRIVE_DOWN 1
if (!REMOTE_isOn) {
return;
}
if (z<-900) { /* have a way to stop motor: turn FRDM USB port side up or down */
#if PL_CONFIG_HAS_DRIVE
DRV_SetSpeed(0, 0);
#else
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
#endif
} else if ((directionVal>MIN_VALUE || directionVal<-MIN_VALUE) && (speedVal>MIN_VALUE || speedVal<-MIN_VALUE)) {
int16_t speed, speedL, speedR;
speed = speedVal/SCALE_DOWN;
if (directionVal<0) {
if (speed<0) {
speedR = speed+(directionVal/SCALE_DOWN);
} else {
speedR = speed-(directionVal/SCALE_DOWN);
}
speedL = speed;
} else {
speedR = speed;
if (speed<0) {
speedL = speed-(directionVal/SCALE_DOWN);
} else {
speedL = speed+(directionVal/SCALE_DOWN);
}
}
#if PL_CONFIG_HAS_DRIVE
DRV_SetSpeed(speedL*SCALE_DOWN/DRIVE_DOWN, speedR*SCALE_DOWN/DRIVE_DOWN);
#else
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), speedL);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), speedR);
#endif
} else if (speedVal>100 || speedVal<-100) { /* speed */
#if PL_CONFIG_HAS_DRIVE
if(nitro){
DRV_SetSpeed((nitrovalue+2)*speedVal, (nitrovalue+2)*speedVal);
} else {
DRV_SetSpeed(3*speedVal, 3*speedVal);
}
#else
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), -speedVal/SCALE_DOWN);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), -speedVal/SCALE_DOWN);
#endif
} else if (directionVal>100 || directionVal<-100) { /* direction */
#if PL_CONFIG_HAS_DRIVE
if(nitro){
DRV_SetSpeed((1+nitrovalue)*directionVal, -directionVal*(1+nitrovalue));
} else {
DRV_SetSpeed(2*directionVal, -directionVal*2);
}
#else
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), -directionVal/SCALE_DOWN);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), (directionVAl/SCALE_DOWN));
#endif
} else { /* device flat on the table? */
#if PL_CONFIG_HAS_DRIVE
DRV_SetSpeed(0, 0);
#else
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
#endif
}
}
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 */
}
void TURN_Turn(TURN_Kind kind) {
switch(kind) {
case TURN_LEFT45:
#if PL_HAS_QUADRATURE
Turn(TRUE, 0, TURN_Steps90/2);
#else
Turn(TRUE, TURN_DutyPercent, TURN_Time90ms/2);
#endif
break;
case TURN_RIGHT45:
#if PL_HAS_QUADRATURE
Turn(FALSE, 0, TURN_Steps90/2);
#else
Turn(FALSE, TURN_DutyPercent, TURN_Time90ms/2);
#endif
break;
case TURN_LEFT90:
#if PL_HAS_QUADRATURE
TURN_TurnSteps(-TURN_Steps90, TURN_Steps90);
#else
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), -TURN_DutyPercent);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), TURN_DutyPercent);
WAIT1_WaitOSms(TURN_Time90ms); /* only use waiting time */
#endif
#if TURN_STOP_AFTER_TURN
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
#if TURN_WAIT_AFTER_STEP_MS > 0
WAIT1_WaitOSms(TURN_WAIT_AFTER_STEP_MS);
#endif
#endif
break;
case TURN_RIGHT90:
#if PL_HAS_QUADRATURE
TURN_TurnSteps(TURN_Steps90, -TURN_Steps90);
#else
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), TURN_DutyPercent);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), -TURN_DutyPercent);
WAIT1_WaitOSms(TURN_Time90ms); /* only use waiting time */
#endif
#if TURN_STOP_AFTER_TURN
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
#if TURN_WAIT_AFTER_STEP_MS > 0
WAIT1_WaitOSms(TURN_WAIT_AFTER_STEP_MS);
#endif
#endif
break;
case TURN_LEFT180:
#if PL_HAS_QUADRATURE
TURN_TurnSteps(-(2*TURN_Steps90), 2*TURN_Steps90);
#else
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), -TURN_DutyPercent);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), TURN_DutyPercent);
WAIT1_WaitOSms(2*TURN_Time90ms);
#endif
#if TURN_STOP_AFTER_TURN
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
#if TURN_WAIT_AFTER_STEP_MS > 0
WAIT1_WaitOSms(TURN_WAIT_AFTER_STEP_MS);
#endif
#endif
break;
case TURN_RIGHT180:
#if PL_HAS_QUADRATURE
TURN_TurnSteps(2*TURN_Steps90, -(2*TURN_Steps90));
#else
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), TURN_DutyPercent);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), -TURN_DutyPercent);
WAIT1_WaitOSms(2*TURN_Time90ms);
#endif
#if TURN_STOP_AFTER_TURN
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
#if TURN_WAIT_AFTER_STEP_MS > 0
WAIT1_WaitOSms(TURN_WAIT_AFTER_STEP_MS);
#endif
#endif
break;
case TURN_STEP_FW:
#if PL_HAS_QUADRATURE
TURN_TurnSteps(TURN_StepsFwBw, TURN_StepsFwBw);
#else
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), TURN_DutyPercent);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), TURN_DutyPercent);
WAIT1_WaitOSms(TURN_StepFwBwMs);
#endif
#if TURN_STOP_AFTER_TURN
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
#if TURN_WAIT_AFTER_STEP_MS > 0
WAIT1_WaitOSms(TURN_WAIT_AFTER_STEP_MS);
#endif
#endif
break;
case TURN_STEP_BW:
#if PL_HAS_QUADRATURE
TURN_TurnSteps(-TURN_StepsFwBw, -TURN_StepsFwBw);
#else
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), -TURN_DutyPercent);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), -TURN_DutyPercent);
WAIT1_WaitOSms(TURN_StepFwBwMs);
#endif
#if TURN_STOP_AFTER_TURN
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
#if TURN_WAIT_AFTER_STEP_MS > 0
WAIT1_WaitOSms(TURN_WAIT_AFTER_STEP_MS);
#endif
#endif
break;
case TURN_STEP_BW_SMALL:
#if PL_HAS_QUADRATURE
TURN_TurnSteps(-TURN_StepsFwBw/2, -TURN_StepsFwBw/2);
#else
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), -TURN_DutyPercent);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), -TURN_DutyPercent);
WAIT1_WaitOSms(TURN_StepFwBwMs/2);
#endif
#if TURN_STOP_AFTER_TURN
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
#if TURN_WAIT_AFTER_STEP_MS > 0
WAIT1_WaitOSms(TURN_WAIT_AFTER_STEP_MS);
#endif
#endif
break;
case TURN_STOP:
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
#if TURN_WAIT_AFTER_STEP_MS > 0
WAIT1_WaitOSms(TURN_WAIT_AFTER_STEP_MS);
#endif
break;
default:
break;
}
}
void TURN_Turn(TURN_Kind kind, bool toLine) {
#if 0
uint16_t lineVal;
bool online;
#endif
switch(kind) {
case TURN_LEFT90:
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), -TURN_DutyPercent);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), TURN_DutyPercent);
#if 0
if (toLine) {
WAIT1_WaitOSms(TURN_TimeMs*2/3); /* turn half way */
do {
lineVal = REF_GetLineValue(&online);
} while(lineVal>REF_MIDDLE_LINE_VALUE);
} else {
WAIT1_WaitOSms(TURN_TimeMs); /* only use waiting time */
}
#else
WAIT1_WaitOSms(TURN_TimeMs); /* only use waiting time */
#endif
#if TURN_STOP_AFTER_TURN
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
#if TURN_WAIT_AFTER_STEP_MS > 0
WAIT1_WaitOSms(TURN_WAIT_AFTER_STEP_MS);
#endif
#endif
break;
case TURN_RIGHT90:
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), TURN_DutyPercent);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), -TURN_DutyPercent);
#if 0
if (toLine) {
WAIT1_WaitOSms(TURN_TimeMs*2/3); /* turn half way */
do {
lineVal = REF_GetLineValue(&online);
} while(lineVal<REF_MIDDLE_LINE_VALUE);
} else {
WAIT1_WaitOSms(TURN_TimeMs); /* only use waiting time */
}
#else
WAIT1_WaitOSms(TURN_TimeMs); /* only use waiting time */
#endif
#if TURN_STOP_AFTER_TURN
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
#if TURN_WAIT_AFTER_STEP_MS > 0
WAIT1_WaitOSms(TURN_WAIT_AFTER_STEP_MS);
#endif
#endif
break;
case TURN_LEFT180:
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), -TURN_DutyPercent);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), TURN_DutyPercent);
WAIT1_WaitOSms(2*TURN_TimeMs);
#if TURN_STOP_AFTER_TURN
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
#if TURN_WAIT_AFTER_STEP_MS > 0
WAIT1_WaitOSms(TURN_WAIT_AFTER_STEP_MS);
#endif
#endif
break;
case TURN_RIGHT180:
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), TURN_DutyPercent);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), -TURN_DutyPercent);
WAIT1_WaitOSms(2*TURN_TimeMs);
#if TURN_STOP_AFTER_TURN
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
#if TURN_WAIT_AFTER_STEP_MS > 0
WAIT1_WaitOSms(TURN_WAIT_AFTER_STEP_MS);
#endif
#endif
break;
case TURN_STEP_FW:
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), TURN_DutyPercent);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), TURN_DutyPercent);
WAIT1_WaitOSms(TURN_StepMs);
#if TURN_STOP_AFTER_TURN
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
#if TURN_WAIT_AFTER_STEP_MS > 0
WAIT1_WaitOSms(TURN_WAIT_AFTER_STEP_MS);
#endif
#endif
break;
case TURN_STEP_BW:
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), -TURN_DutyPercent);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), -TURN_DutyPercent);
WAIT1_WaitOSms(TURN_StepMs);
#if TURN_STOP_AFTER_TURN
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
#if TURN_WAIT_AFTER_STEP_MS > 0
WAIT1_WaitOSms(TURN_WAIT_AFTER_STEP_MS);
#endif
#endif
break;
case TURN_STOP:
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
#if TURN_WAIT_AFTER_STEP_MS > 0
WAIT1_WaitOSms(TURN_WAIT_AFTER_STEP_MS);
#endif
break;
default:
break;
}
}
static portTASK_FUNCTION(DriveTask, pvParameters) {
(void)pvParameters; /* parameter not used */
bool prevOnS, prevOnP;
uint32_t i;
int32_t PosL,PosR;
uint32_t dL,dR;
uint32_t sL,sR;
//traceLabel usrEvent;
prevOnS = DRV_SpeedOn;
prevOnP = DRV_PosOn;
sL = 30000;
sR = 30000;
#if PL_HAS_RTOS_TRACE
//usrEvent = xTraceOpenLabel("drive");
#endif
for(;;) {
#if PL_HAS_RTOS_TRACE
//RTOSTRC1_vTraceUserEvent(usrEvent);
#endif
/*! \todo extend this for your own needs and with a position PID */
TACHO_CalcSpeed();
if (prevOnS && !DRV_SpeedOn) { /* speed turned off */
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
PID_Start(); /* reset values */
} else if (DRV_SpeedOn && !DRV_PosOn) { /*speed control*/
PID_Speed(TACHO_GetSpeed(TRUE), DRV_SpeedLeft, TRUE); /* left */
PID_Speed(TACHO_GetSpeed(FALSE), DRV_SpeedRight, FALSE); /* right */
} else if (DRV_PosOn && !DRV_SpeedOn) { /*pos control*/
PosL = Q4CLeft_GetPos();
PosR = Q4CRight_GetPos();
dL = ((PosL-DRV_PosLeft)<=0) ? -(PosL-DRV_PosLeft) : (PosL-DRV_PosLeft);
dR = ((PosR-DRV_PosRight)<=0) ? -(PosR-DRV_PosRight) : (PosR-DRV_PosRight);
if(dL>100){
if((DRV_PosLeft-PosL)>0){
sL += 15;
} else{
sL -= 15;
}
} else{
sL = DRV_PosLeft;
}
if(dR>100){
if((DRV_PosRight-PosR)>0){
sR += 15;
} else{
sR -= 15;
}
} else{
sR = DRV_PosRight;
}
PID_Pos(PosL, sL, TRUE); //left
PID_Pos(PosR, sR, FALSE); //right
if((PosR < 1000) || (PosR > 63000)){ /*range safety*/
DRV_PosRight = 30000;
Q4CRight_SetPos(30000);
}
if((PosL < 1000) || (PosL > 63000)){
DRV_PosLeft = 30000;
Q4CLeft_SetPos(30000);
}
} else if(prevOnP && !DRV_PosOn){ /*pos turned off*/
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
for(i=0;i<100000;i++);
DRV_PosLeft = 30000;
DRV_PosRight = 30000;
Q4CLeft_SetPos(30000);
Q4CRight_SetPos(30000);
}
prevOnS = DRV_SpeedOn;
prevOnP = DRV_PosOn;
FRTOS1_vTaskDelay(2/portTICK_RATE_MS);
} /* for */
}
static void REMOTE_HandleMotorMsg(int16_t speedVal, int16_t directionVal, int16_t z) {
#define MIN_VALUE 350 /* old: 250 - values below this value are ignored */
#define Nitro 2
#define Zuercher 4 /* gerade aus -> schnell */
if (!REMOTE_isOn) {
return;
}
if (z<-900) { /* have a way to stop motor: turn FRDM USB port side up or down */
#if PL_CONFIG_HAS_DRIVE
DRV_SetSpeed(0, 0);
#else
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
#endif
} else if ((directionVal>MIN_VALUE || directionVal<-MIN_VALUE) && (speedVal>MIN_VALUE || speedVal<-MIN_VALUE)) {
int16_t speed, speedL, speedR;
speed = speedVal*2;
if (directionVal<0) {
if (speed<0) {
speedR = speed+(directionVal);
} else {
speedR = speed-(directionVal);
}
speedL = speed;
} else {
speedR = speed;
if (speed<0) {
speedL = speed-(directionVal);
} else {
speedL = speed+(directionVal);
}
}
#if PL_CONFIG_HAS_DRIVE
if(NITRO){
DRV_SetSpeed(speedL*Nitro, speedR*Nitro);
}else {
DRV_SetSpeed(speedL, speedR);
}
#else
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), speedL);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), speedR);
#endif
} else if (speedVal>100 || speedVal<-100) { /* speed */
#if PL_CONFIG_HAS_DRIVE
if(NITRO){
DRV_SetSpeed(speedVal*3*Nitro, speedVal*3*Nitro); /* Zuercher & Nitro esch chli vell */
} else {
DRV_SetSpeed(speedVal*Zuercher, speedVal*Zuercher);
}
#else
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), -speedVal/SCALE_DOWN);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), -speedVal/SCALE_DOWN);
#endif
} else if (directionVal>110 || directionVal<-110) { /* direction */
#if PL_CONFIG_HAS_DRIVE
if(NITRO){
DRV_SetSpeed(directionVal*Nitro, -directionVal*Nitro);
} else {
DRV_SetSpeed(directionVal, -directionVal);
}
#else
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), -directionVal/SCALE_DOWN);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), (directionVAl/SCALE_DOWN));
#endif
} else { /* device flat on the table? */
#if PL_CONFIG_HAS_DRIVE
DRV_SetSpeed(0, 0);
#else
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
#endif
}
}
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);
}
static portTASK_FUNCTION(Remote, pvParameters) {
int32_t valMotR = 0;
int32_t valMotL = 0;
for(;;) {
#if PL_HAS_REMOTE && PL_IS_FRDM
protocol42 txdata;
if (analogOn) {
GetXY(&x_anal,&y_anal);
txdata.target = isROBOcop;
txdata.type = anal_x;
txdata.data = x_anal;
sendData42(txdata);
txdata.type = anal_y;
txdata.data = y_anal;
sendData42(txdata);
}
if (accelOn){
x_ac = MMA1_GetXmg();
y_ac = MMA1_GetYmg();
x_accel = scaleFromAccelToU8(x_ac);
y_accel = scaleFromAccelToU8(y_ac);
txdata.target = isROBOcop;
txdata.type = accel_x;
txdata.data = x_accel;
sendData42(txdata);
txdata.type = accel_y;
txdata.data = y_accel;
sendData42(txdata);
}
#endif
#if PL_HAS_MOTOR
if(valX > 0) {
valMotR = valY-valX;
valMotL = valY;
}
else if (valX < 0) {
valMotR = valY;
valMotL = valY+valX;
}
else {
valMotR = valY;
valMotL = valY;
}
if((valY < 3) && (valY > -3)) {
valMotR = 0;
valMotL = 0;
}
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), valMotR);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT) , valMotL);
#endif
FRTOS1_vTaskDelay(20/TRG_TICKS_MS);
}
}
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
}
static portTASK_FUNCTION(TraceTask, pvParameters) {
unsigned char buf[32];
(void)pvParameters;
//ACCEL_LowLevelInit(); /* cannot do this in ACCEL_Init(), as there interrupts are disabled */
for(;;) {
if (traceChannel==TRACE_TO_NONE) {
/* do nothing */
} else if (traceChannel==TRACE_TO_SHELL) {
buf[0] = '\0';
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" => ");
#if PL_HAS_MOTOR
if (traceMotor) {
buf[0] = '\0';
if (MOT_GetDirection(MOT_GetMotorHandle(MOT_MOTOR_LEFT))==MOT_DIR_FORWARD) {
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" M: fw 0x");
} else {
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" M: bw 0x");
}
UTIL1_strcatNum16Hex(buf, sizeof(buf), MOT_GetVal(MOT_GetMotorHandle(MOT_MOTOR_LEFT)));
UTIL1_chcat(buf, sizeof(buf), ';');
#if PL_HAS_SHELL
SHELL_SendString(&buf[0]);
#endif
}
#endif
#if PL_HAS_MOTOR_TACHO
if (traceTacho) {
buf[0] = '\0';
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" V:");
UTIL1_strcatNum32sFormatted(buf, sizeof(buf), TACHO_GetSpeed(FALSE), ' ', 6);
UTIL1_chcat(buf, sizeof(buf), ' ');
UTIL1_strcatNum32sFormatted(buf, sizeof(buf), TACHO_GetSpeed(TRUE), ' ', 6);
UTIL1_chcat(buf, sizeof(buf), ';');
#if PL_HAS_SHELL
SHELL_SendString(&buf[0]);
#endif
}
#endif
#if PL_HAS_QUADRATURE
if (traceQuad) {
buf[0] = '\0';
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" P:");
UTIL1_strcatNum16uFormatted(buf, sizeof(buf), Q4CLeft_GetPos(), ' ', 6);
UTIL1_strcatNum16uFormatted(buf, sizeof(buf), Q4CRight_GetPos(), ' ', 6);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"; E:");
UTIL1_strcatNum16uFormatted(buf, sizeof(buf), Q4CLeft_NofErrors(), ' ', 6);
UTIL1_strcatNum16uFormatted(buf, sizeof(buf), Q4CRight_NofErrors(), ' ', 6);
UTIL1_chcat(buf, sizeof(buf), ';');
#if PL_HAS_SHELL
SHELL_SendString(&buf[0]);
#endif
}
#endif
#if PL_HAS_ACCEL
if (traceAccel) {
int16_t x, y, z;
buf[0] = '\0';
ACCEL_GetValues(&x, &y, &z);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)" X:");
UTIL1_strcatNum16sFormatted(buf, sizeof(buf), x, ' ', 6);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"; Y:");
UTIL1_strcatNum16sFormatted(buf, sizeof(buf), y, ' ', 6);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"; Z:");
UTIL1_strcatNum16sFormatted(buf, sizeof(buf), z, ' ', 6);
UTIL1_chcat(buf, sizeof(buf), ';');
#if PL_HAS_SHELL
SHELL_SendString(&buf[0]);
#endif
}
#endif
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"\r\n");
#if PL_HAS_SHELL
SHELL_SendString(&buf[0]);
#endif
FRTOS1_vTaskDelay(20/portTICK_RATE_MS); /* slow down writing to console */
}
FRTOS1_vTaskDelay(20/portTICK_RATE_MS);
} /* for */
}
