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