void arcade_drive_routine(void)
{
char left_power,
right_power,
impeller_power,
arm_power,
joy_x,
joy_y;
joy_x = rc_read_data(ARCADE_DRIVE_X_CHAN);
joy_y = -rc_read_data(ARCADE_DRIVE_Y_CHAN);
arcade_drive(joy_x, joy_y, PWM_MAX, &left_power, &right_power);
// printf("%d %d %d %d\n", joy_x, joy_y, left_power, right_power);
/* Drive motors */
pwm_write(LEFT_DRIVE_PORT, -left_power);
pwm_write(RIGHT_DRIVE_PORT, right_power);
/* Implements */
impeller_power = rc_read_data(ARCADE_IMPELLER_CHAN);
arm_power = rc_read_data(ARCADE_ARM_CHAN);
pwm_write(IMPELLER_PORT, impeller_power);
pwm_write(ARM_PORT, arm_power);
}
task drive() {
while(true){
getJoystickSettings(joystick);
//these have to be floored to create a dead zone, preventing motor drift and noise
forward = floor(joystick.joy1_y1, 10) * current_power_level;
turn = floor(joystick.joy1_x2,10) * current_power_level;
if (abs(forward) < 10){
forward = 0;
}
arcade_drive(forward, turn);
nxtDisplayTextLine(1, "forward: %d", forward);
nxtDisplayTextLine(2, "turn: %d", turn);
}
}
void arcade_drive_routine(void)
{
static char rc_pos,
implement_pos = SERVO_CENTER;
static char left_drive,
right_drive,
joy_x,
joy_y;
joy_x = rc_read_data(ARCADE_DRIVE_X_CHAN);
joy_y = -rc_read_data(ARCADE_DRIVE_Y_CHAN);
arcade_drive(joy_x, joy_y, PWM_MAX, &left_drive, &right_drive);
// printf("%d %d %d %d\n", joy_x, joy_y, left_drive, right_drive);
/*
* This code is for a forklift implement mounted on a servo.
* If forklift is below level, digitally "gear down" the drive motors
* for finer control of the robot while trying to slide forks
* under a pallet.
*/
if ( implement_pos > SERVO_CENTER )
{
left_drive /= IMPLEMENT_GEAR_DOWN;
right_drive /= IMPLEMENT_GEAR_DOWN;
}
/* Drive motors */
pwm_write(LEFT_DRIVE_PORT, -left_drive);
pwm_write(RIGHT_DRIVE_PORT, right_drive);
/*
* Servo controlling implement on port 4
* If joystick is off center by the
* threshold value (to avoid moving due to joystik trim issues and
* inadvertent lateral movements) and the servo
* position is within bounds, move servo one position up or down.
*/
rc_pos = -rc_read_data(ARCADE_IMPLEMENT_CHAN);
if ( (rc_pos < JOY_CENTER - JOY_THRESHOLD) &&
(implement_pos < IMPLEMENT_DOWN) )
++implement_pos;
else if ( (rc_pos > JOY_CENTER + JOY_THRESHOLD) &&
(implement_pos > IMPLEMENT_UP) )
--implement_pos;
pwm_write(IMPLEMENT_SERVO_PORT, implement_pos);
}
