static uint8_t Tune(const CLS1_StdIOType *io, uint8_t channel, MOT_MotorDevice *motorHandle) {
#define TUNE_MOTOR_PERCENT 20
uint16_t dac;
int i;
QuadTime_t timing;
uint8_t buf[48];
uint8_t res;
//#if PL_HAS_DRIVE
// DRV_SetMode(DRV_MODE_NONE); /* turn off drive mode */
//#endif
MOT_SetSpeedPercent(motorHandle, TUNE_MOTOR_PERCENT);
CLS1_SendStr((uint8_t*)"Tuning channel...\r\n", io->stdOut);
res = ERR_FAILED;
for(i=0,dac=0;dac<=MCP4728_MAX_DAC_VAL;i++) {
UTIL1_strcpy(buf, sizeof(buf), (uint8_t*)"Channel: ");
UTIL1_chcat(buf, sizeof(buf), (uint8_t)('A'+channel)); /* 0:A, 1:B, 2:C, 3:D */
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);
if (MCP4728_FastWriteDAC(channel, dac)!=ERR_OK) { /* writes single channel DAC value, not updating EEPROM */
CLS1_SendStr((uint8_t*)"ERROR writing DAC channel!\r\n", io->stdErr);
res = ERR_FAILED;
break;
}
WAIT1_WaitOSms(100); /* wait some time to allow DAC and OP-Amp 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);
CLS1_SendStr((uint8_t*)"Writing to EEPROM...\r\n", io->stdOut);
if (MCP4728_WriteDACandEE(channel, dac)!=ERR_OK) {
CLS1_SendStr((uint8_t*)"ERROR writing DAC/EEPROM\r\n", io->stdErr);
res = ERR_FAILED;
break;
}
CLS1_SendStr((uint8_t*)"...done!\r\n", io->stdOut);
res = ERR_OK;
break; /* go to next channel */
}
dac += 0x1; /* smaller increase */
} else {
CLS1_SendStr((uint8_t*)"No signal\r\n", io->stdErr);
dac += 0x10; /* larger increase */
}
} /* for finding DAC value */
MOT_SetSpeedPercent(motorHandle, 0); /* turn off again */
if (res!=ERR_OK) {
CLS1_SendStr((uint8_t*)"ERROR!\r\n", io->stdErr);
}
CLS1_SendStr((uint8_t*)"Tuning finished!\r\n", io->stdOut);
return res;
}
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;
}
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 MOT_Deinit(void) {
/*! \todo What could you do here? */
MOT_SetSpeedPercent(&motorL, 0);
MOT_SetSpeedPercent(&motorR, 0);
PWML_Disable();
PWMR_Disable();
}
static portTASK_FUNCTION(RoboTask, pvParameters) {
uint16_t cm;
(void)pvParameters; /* not used */
for(;;) {
cm = MeasureCm();
LEDR_Neg();
if (runIt && cm != 0) {
if (cm<10) { /* back up! */
MOT_SetSpeedPercent(MOT_GetMotorA(), -40);
MOT_SetSpeedPercent(MOT_GetMotorB(), -40);
} else if (cm>=10 && cm<=15) {
/* stand still */
MOT_SetSpeedPercent(MOT_GetMotorA(), 0);
MOT_SetSpeedPercent(MOT_GetMotorB(), 0);
} else if (cm>15 && cm<=40) {
MOT_SetSpeedPercent(MOT_GetMotorA(), 50);
MOT_SetSpeedPercent(MOT_GetMotorB(), 50);
} else if (cm>40 && cm<80) {
MOT_SetSpeedPercent(MOT_GetMotorA(), 80);
MOT_SetSpeedPercent(MOT_GetMotorB(), 80);
} else { /* nothing in range */
MOT_SetSpeedPercent(MOT_GetMotorA(), 0);
MOT_SetSpeedPercent(MOT_GetMotorB(), 0);
}
}
FRTOS1_vTaskDelay(50/portTICK_RATE_MS);
}
}
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);
}
}
}
}
void MOT_Deinit(void) {
#if 1
MOT_SetSpeedPercent(&motorL, 0);
MOT_SetSpeedPercent(&motorR, 0);
PWML_Disable();
PWMR_Disable();
#endif
}
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 MOT_Init(void) {
motorL.DirPutVal = DirLPutVal;
motorR.DirPutVal = DirRPutVal;
motorL.SetRatio16 = PWMLSetRatio16;
motorR.SetRatio16 = PWMRSetRatio16;
MOT_SetSpeedPercent(MOT_MOTOR_LEFT, 0);
MOT_SetSpeedPercent(MOT_MOTOR_RIGHT, 0);
PWML_Enable();
PWMR_Enable();
}
void MOT_Init(void) {
#if MOTOR_HAS_INVERT
motorL.inverted = FALSE;
motorR.inverted = FALSE;
#endif
motorL.DirPutVal = DirLPutVal;
motorR.DirPutVal = DirRPutVal;
motorL.SetRatio16 = PWMLSetRatio16;
motorR.SetRatio16 = PWMRSetRatio16;
MOT_SetSpeedPercent(&motorL, 0);
MOT_SetSpeedPercent(&motorR, 0);
(void)PWML_Enable();
(void)PWMR_Enable();
}
void MOT_Init(void) {
#if PL_HAS_MOTOR_BRAKE
motorL.BrakePutVal = BrakeLPutVal;
motorR.BrakePutVal = BrakeRPutVal;
#endif
motorL.DirPutVal = DirLPutVal;
motorR.DirPutVal = DirRPutVal;
motorL.SetRatio16 = PWMLSetRatio16;
motorR.SetRatio16 = PWMRSetRatio16;
MOT_SetSpeedPercent(&motorL, 0);
MOT_SetSpeedPercent(&motorR, 0);
PWML_Enable();
PWMR_Enable();
}
void MOT_ChangeSpeedPercent(MOT_MotorDevice *motor, MOT_SpeedPercent relPercent) {
relPercent += motor->currSpeedPercent; /* make absolute number */
if (relPercent>100) { /* check for overflow */
relPercent = 100;
} else if (relPercent<-100) { /* and underflow */
relPercent = -100;
}
MOT_SetSpeedPercent(motor, relPercent); /* set speed. This will care about the direction too */
}
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
}
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 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
}
}
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
}
}
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 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;
}
}
/* Debugging Function */
void MOT_StopMotors(){
MOT_SetSpeedPercent(&motorL, 0);
MOT_SetSpeedPercent(&motorR, 0);
}
/* Debugging Function */
void MOT_StartMotors(){
MOT_SetSpeedPercent(&motorL, 15);
MOT_SetSpeedPercent(&motorR, 15);
}
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 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;
}
