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; }