task main()
{
wait(3, seconds);
setMultipleMotors(50, motorB, motorC, motorC);
wait(2, seconds);
stopAllMotors();
wait(1, seconds);
turnLeft(3, rotations, 50);
}
void initMotorSystem() {
stopAllMotors();
getAndResetMotorTicks(MOTOR1);
getAndResetMotorTicks(MOTOR2);
getAndResetMotorTicks(MOTOR3);
encoder_accu_rel[0] = 0;
encoder_accu_rel[1] = 0;
encoder_accu_rel[2] = 0;
encoder_rel[0] = 0;
encoder_rel[1] = 0;
encoder_rel[2] = 0;
}
void cn_debug_serial(uword command) {
sword pwm;
unsigned static short int motor = MOTOR3;
switch(command) {
case 'a':
setMotorDirection(motor, DIRECTION_CCW);
break;
case 's':
setMotorDirection(motor, DIRECTION_CW);
break;
case '1':
motor = MOTOR1;
break;
case '2':
motor = MOTOR2;
break;
case '3':
motor = MOTOR3;
break;
case 'u': // PWM up
pwm = getMotorPWM(motor);
pwm += 7;
setMotorPWM(motor, pwm);
break;
case 'd': // PWM down
pwm = getMotorPWM(motor);
pwm -= 7;
if(pwm < ZERO_PWM) pwm = ZERO_PWM;
setMotorPWM(motor, pwm);
break;
case 't': // toggle direction
toggleMotorDirection(motor);
break;
case ' ': // PWM off
stopAllMotors();
break;
default:
break;
}
}
void* hwPump_UART_thread(void* arg)
{
LOG_INFO("hwPump_UART_thread started");
printf("UART pump thread started\n");
// update motor controllers from snapshot whenever we
// are told to do it by egMotorWrite being pulsed.
//
// note: if the event gate times out, the motors are set to coast
// to prevent damage.
//
// time at start of loop
UINT64 usec_loopStart = phiUpTime();
while (TRUE) {
int ctrlID;
int selIdx;
int motorIdx = 0;
//
// Send motor power commands to all controllers
//
// go through each controller
for (ctrlID = 0 ; ctrlID <= 5 ; ctrlID ++) {
// go through each motor of this controller
for (selIdx = 0 ; selIdx <= 1 ; selIdx ++) {
// send out motor power command for this motor
lock_snapshot();
float power = phiSnapshot.cmds.motors[motorIdx++];
unlock_snapshot();
// snapshot power is [-1, 1], convert to [0, 127], bFwd
BYTE absPower = (BYTE) (fabs(power) * 127);
BOOL bFwd = (power >= 0) ? TRUE : FALSE;
HAL_setMotorPower(ctrlID, selIdx, absPower, bFwd);
}
}
// time at end of work
UINT64 usec_workEnd = phiUpTime();
INT32 usec_workTime = (INT32) (usec_workEnd - usec_loopStart);
//
// Wait on event gate to be told data has been updated
//
// The purpose of the event gate is to allow commands to be written to the
// UARTs as quickly as possible after they are received. It is basically an
// interruptible sleep. We wait on the gate but timeout if it takes too long.
// When a timeout occurs, it means we have not received commands from the
// server for a while and something is seriously wrong. In this case, we
// stop all motors for safety.
#define MOTOR_TIMEOUT_MS 500 // 1/2 second
int waitRc = eventGate_wait(&egMotorWrite, MOTOR_TIMEOUT_MS);
if (waitRc == ETIMEDOUT) {
// gate timed out - only bad if link connected
if (g_phiLinkStatus == LINK_CONNECTED) {
// link is connected - stop all motors
stopAllMotors();
// count it
numMotorTimeouts ++;
// DEBUG
LOG_INFO("eventGate timeout in UART pump - count=%lu", numMotorTimeouts);
}
}
// new loop start
usec_loopStart = phiUpTime();
// log loop period
if (UART_loopStart_save != 0) {
dlog_addElem(g_pDlog_hwPump_UART_period, usec_loopStart-UART_loopStart_save);
}
UART_loopStart_save = usec_loopStart;
// log loop work time
dlog_addElem(g_pDlog_hwPump_UART_workTime, usec_workTime);
} // while
} // hwPump_UART_thread
