/*
* Slot wrapper for MobilePlatform::motionStop()
* @brief MobilePlatform::slot_stopMotion()
*/
void MobilePlatform::slot_stopMotion()
{
m_mutex.lock();
m_iCircleRadius = 0;
m_bendReached = false;
motionStop();
m_mutex.unlock();
}
// the stepper thread processes incoming setpoint commands in order of arrival
// there's no lookahead, just take each command and do it as well as possible
static msg_t stepperTh (void*) {
systime_t timeout = TIME_IMMEDIATE;
for (;;) {
msg_t spIndex;
if (chMBFetch(&setpoint.mailbox, &spIndex, timeout) == RDY_OK) {
#ifdef GPIO1_OLI_LED1
palClearPad(GPIO1, GPIO1_OLI_LED1); // active low, on
#endif
motionTarget(setpoint.setpoints[spIndex]);
clearInUse(spIndex);
timeout = TIME_IMMEDIATE;
} else {
// there is no work, stop the stepper and wait for next cmd
motionStop();
#ifdef GPIO1_OLI_LED1
palSetPad(GPIO1, GPIO1_OLI_LED1); // active low, off
#endif
timeout = TIME_INFINITE;
}
}
return 0;
}
/*
* Constructor of the class MobilePlatform
* @brief MobilePlatform::MobilePlatform
*/
MobilePlatform::MobilePlatform()
{
m_iCircleRadius = 17;
m_dCircleSpeed = 0.3;
//Initialize and start thread for the class MobilePlatform
moveToThread(&m_thread);
m_thread.start();
//Create two objects of the type Dcmotor
m_pMotorLeft = new Dcmotor(MOTOR_LEFT_FORWARD, MOTOR_LEFT_BACKWARD, MOTOR_LEFT_PWM, ENCODER_LEFT_A, ENCODER_LEFT_B);
m_pMotorRight = new Dcmotor(MOTOR_RIGHT_FORWARD, MOTOR_RIGHT_BACKWARD, MOTOR_RIGHT_PWM, ENCODER_RIGHT_A, ENCODER_RIGHT_B);
//Create two objects of the type Linesensor
m_pLineSensorRight = new Linesensor(LINESENSOR_RIGHT);
m_pLineSensorLeft = new Linesensor(LINESENSOR_LEFT);
//Stop both dc motors
motionStop();
}
