void VehicleSetSteeringWheel(int32_t adj)
{
if (bVehicleAuto) return;
int throttle = MOTOR_RIGHT;
if (adj < 0) {
throttle = MOTOR_LEFT;
adj = 0 - adj;
}
float factor = 1.0f;
if (adj > 8) { /* 9, 10 */
factor = -1.0f;
} else if (adj > 5) { /* 6, 7, 8 */
factor = 0.0f;
} else if (adj > 2) { /* 3, 4, 5 */
factor = 0.6f;
} else if (adj > 0) { /* 1, 2 */
factor = 0.9f;
}
MotorRun(MOTOR_LEFT , (throttle == MOTOR_LEFT )? (iVehicleMotorDutyL * factor) : iVehicleMotorDutyL);
MotorRun(MOTOR_RIGHT, (throttle == MOTOR_RIGHT)? (iVehicleMotorDutyR * factor) : iVehicleMotorDutyR);
}
void CDlgMotorMontior::OnLCount()
{
if(!m_Operator)
return;
if(Motor->DRV)
{
AfxMessageBox("Please wait for motor stop.");
return;
}
CDlgMotorMove dlg(Motor->LCount);
if(dlg.DoModal() != IDOK) return;
m_bStop = false;
bool bOK = false;
bOK = MotorRun(dlg.m_NewGo);
}
static void vTaskVehicleCtrl(void *pvParameters)
{
float heading;
while (1) {
if (bVehicleAuto) {
__WFI();
int angle = angle_subtract(heading, CompassGetHeading());
if (angle < 90) {
MotorRun(MOTOR_LEFT , iVehicleMotorDutyL);
MotorRun(MOTOR_RIGHT, -iVehicleMotorDutyR);
continue;
}
if (angle > 100) {
MotorRun(MOTOR_LEFT , -iVehicleMotorDutyL);
MotorRun(MOTOR_RIGHT, iVehicleMotorDutyR);
continue;
}
MotorRun(MOTOR_LEFT , iVehicleMotorDutyL);
MotorRun(MOTOR_RIGHT, iVehicleMotorDutyR);
bVehicleAuto = false;
} else {
Board_LED_Toggle(0);
float obstacle = UltrasonicPing();
if (obstacle < THRESHOLD_AUTODRIVE) {
Board_LED_Set(0, false);
bVehicleAuto = true;
// FIXME
heading = CompassGetHeading();
MotorRun(MOTOR_LEFT , MAX_PWM_CYCLE / 10);
MotorRun(MOTOR_RIGHT, -MAX_PWM_CYCLE / 10);
} else {
/* toggle rate based on the range to the obstacle */
vTaskDelay(200); //beatrate(UltrasonicPing()));
}
}
}
}
void MIYbot::motorStop(){
MotorRun(2,0,true);
MotorRun(3,0,true);
}
void MIYbot::spin(int duration, boolean reverse){
MotorRun(2,SLOW,reverse);
MotorRun(3,SLOW,! reverse);
delay(duration);
motorStop();
}
void MIYbot::reverse(int pwm_speed){
MotorRun(2,pwm_speed,false);
MotorRun(3,pwm_speed,false);
}
//movement
void MIYbot::forward(int pwm_speed){
MotorRun(2,pwm_speed,true);
MotorRun(3,pwm_speed,true);
}
void ButtonGuardThread(void* pvParameters)
{
unsigned long ulTick = 0;
s_ulIdleTick = 0;
unsigned long ulLongKeepOff = 0;
unsigned long ulChrgTick = 0;
bool bEnableRunning = true;
while(1) {
ulTick++;
// 按钮状态检测
int nBtnAction = ButtonCheck();
if (nBtnAction != BTN_NOACTION) {
s_ulIdleTick = 0;
if ( (nBtnAction==BTN_PUSHDOWN) && g_bRunning ) {
g_bRunning = false;
MotorRun(g_bRunning);
bEnableRunning = false;
}
else if ( (nBtnAction==BTN_POPUP) && !g_bRunning ) {
if (!bEnableRunning) {
bEnableRunning = true;
}
else {
if (g_bRunable) { // 如果不在SimpleLink操作阶段则不能运行探头
g_bRunning = true;
MotorRun(g_bRunning);
}
}
}
ulLongKeepOff = 0; // 长按关机计数
} else { // 长按关机状态下关闭电源
if ( s_nStatOut == BTN_UP) {
ulLongKeepOff = 0;
} else {
++ulLongKeepOff;
if (ulLongKeepOff/100 > 8) { // 长按8秒关闭电源
MotorRun(false);
TurnOff();
}
}
// 注：
// 长按关机是为了避免运输工程中，如果探头的按钮受到挤压而开机；
// 在这种状态下，按钮控制芯片并不会在一定时间后主动关闭电源，从而
// 会导致电源耗尽或者其他潜在风险。
// 故而解决的方法是检测按钮是否长时间（8S）处于未知或者按下
// 状态，一旦检测到则关闭电源。
// 吴文斌
// 2015 - 3 - 30
}
// 充电状态检测
if (IsCharging()) {
ulChrgTick++;
if (ulChrgTick > 20) {
TRACE("Begin Charging...\n\r");
g_bRunning = false;
MotorRun(g_bRunning);
TurnOff();
}
}
else {
ulChrgTick = 0;
}
// 待机时间检测
++s_ulIdleTick;
if ( (s_ulIdleTick/100/60 > 10) // 连续运行10分钟则停止运行
&& g_bRunning )
{
TRACE("10min to Stop Running...\n\r");
g_bRunning = !g_bRunning;
MotorRun(g_bRunning);
}
if ( s_ulIdleTick/100/60 > 15) { // 待机时间超过15分钟则关闭电源
TRACE("15min to Power Off...\n\r");
TurnOff();
}
vTaskDelay(10/portTICK_PERIOD_MS);
}
}