'Drive Software 'version 1.00 - intial programming, basically copied from flow chart ' note: constants with an unknown values are set at 1000 so that the syntax czech doesn't go berserk 'version 2.00 - adjusted algorithms to prevent such occurences as (-10) becoming 245 ' corrected faulty axis 1 calibration algorithm 'version 3.00 - determined and set variable sizes ' rewrote drive shift routine so that the "~" symbol is no longer there and the "oppositizing" ' command is replaced with an if, a gosub, and 2 subroutines 'version 4.00 - set joystick inputs to variables used in program ' set turbo input so that one or two turbo buttons can be pressed and it works ' (based on assumption that turbo buttons are on joysticks) ' reset turbo variable size to account for new turbo pressing system ' left code open so that turbo button stuff can be redone if needed 'version 5.00 - changed many gosub's to goto's, got rid of drv_gear_shift, now drv_main,put back~,added step skipped in start shift n=drv_shift-con ' changed shifting to return, and basically condensed to "intentionally fall through" 'version 6.00 - changed drv_axis_1 to inp_joy_1 and drv_axis_2 to inp_joy_2 because carl and sean are stupid idiots who are going to get beaten up (carl created a whitepaper with variables different from the ones previously used in this program and so molly is going to beat him up because of all the work we had to do to change it) 'version 7.00 - changed varaibles in program to be consistent with whitepaper '----------------------Constants------------------- DRV_STOP_CON CON 20 DRV_SHIFT_CON CON 20 DRV_MOTOR_STOP CON 127 DRV_JOY_MAX_RANGE CON 128 DRV_JOY_MIN_RANGE CON 127 DRV_JOY_CENTER CON 127 DRV_CALIB_1_MAX CON 1000 DRV_CALIB_1_MIN CON 1000 DRV_CALIB_1_REST CON 1000 DRV_CALIB_REST_Y_2 CON 1000 DRV_CALIB_Y_2_MAX CON 1000 DRV_CALIB_Y_2_MIN CON 1000 DRV_CALIB_REST_X_2 CON 1000 DRV_CALIB_X_2_MAX CON 1000 DRV_CALIB_X_2_MIN CON 1000 DRV_BUFFER CON 10 DRV_AXIS_1_UPPER_BUFFER CON DRV_CALIB_1_REST + DRV_BUFFER DRV_AXIS_1_LOWER_BUFFER CON DRV_CALIB_1_REST - DRV_BUFFER DRV_AXIS_2_X_LEFT_BUFFER CON DRV_CALIB_REST_X_2 + DRV_BUFFER DRV_AXIS_2_X_RIGHT_BUFFER CON DRV_CALIB_REST_X_2 - DRV_BUFFER DRV_AXIS_2_Y_UPPER_BUFFER CON DRV_CALIB_REST_Y_2 + DRV_BUFFER DRV_AXIS_2_Y_LOWER_BUFFER CON DRV_CALIB_REST_Y_2 - DRV_BUFFER DRV_CALIB_1_UPPER_RANGE CON DRV_CALIB_1_MAX - DRV_AXIS_1_UPPER_BUFFER DRV_CALIB_1_LOWER_RANGE CON DRV_AXIS_1_LOWER_BUFFER - DRV_CALIB_1_MIN DRV_CALIB_2_X_LEFT_RANGE CON DRV_CALIB_X_2_MAX - DRV_AXIS_2_X_LEFT_BUFFER DRV_CALIB_2_X_RIGHT_RANGE CON DRV_AXIS_2_X_RIGHT_BUFFER - DRV_CALIB_X_2_MIN DRV_CALIB_2_Y_UPPER_RANGE CON DRV_CALIB_Y_2_MAX - DRV_AXIS_2_Y_UPPER_BUFFER DRV_CALIB_2_Y_LOWER_RANGE CON DRV_AXIS_2_Y_LOWER_BUFFER - DRV_CALIB_Y_2_MIN DRV_SHIFT_HIGH CON 1 DRV_SHIFT_LOW CON 0 DRV_ONE_JOYSTICK CON 0 DRV_TWO_JOYSTICK CON 1 DRV_TURBO_OFF CON 0 DRV_TURBO_ON CON 1 DRV_SERVO_LOW CON 1000 DRV_SERVO_HIGH CON 1000 DRV_FINE_ON CON 1 DRV_FINE_OFF CON 0 DRV_FINE_CON_MUL CON 1 DRV_FINE_CON_DIV CON 2 DRV_SCAL_MUL CON 4 DRV_SCAL_DIV CON 5 '--------------------variables----------------------- drv_joystick_switch VAR bit drv_n VAR byte drv_shift VAR bit drv_prev_shift VAR bit inp_joy_1 VAR byte inp_joy_2 VAR byte out_drive_right VAR byte out_drive_left VAR byte inp_turbo VAR nib out_servo_right VAR byte inp_drv_fc VAR bit '----------gear shift---------- 'inorder to shift you must make sure the motors are stopped through a loop, 'start shift either high or low, move motors slowly to engage it 'if not have to shift and is stop then allows drive drv_main: if drv_shift = drv_prev_shift then drv_drive_allow_determine if drv_n >= DRV_STOP_CON then drv_start_shift out_drive_right = DRV_MOTOR_STOP out_drive_left = DRV_MOTOR_STOP drv_n = drv_n + 1 return drv_drive_allow_determine: if drv_n = 0 then drv_allow_drive drv_prev_shift = ~drv_shift out_drive_right = DRV_MOTOR_STOP out_drive_left = DRV_MOTOR_STOP drv_n = 1 return drv_start_shift: if drv_shift = DRV_SHIFT_HIGH then drv_set_servo_high out_servo_right = DRV_SERVO_LOW 'intentionally falls through drv_start_shift_back: out_drive_left = DRV_MOTOR_STOP + 15 out_drive_right = DRV_MOTOR_STOP + 15 drv_n = drv_n+1 if drv_n = drv_shift_con then drv_shift_done return drv_set_servo_high: out_servo_right = DRV_SERVO_HIGH goto drv_start_shift_back drv_shift_done: drv_prev_shift = drv_shift drv_n = 0 return '---------------set drive outputs------------ 'sets drive outputs then determines if one or two joysticks 'calibrates accordingly of the 2_x or 2_y and calibrates axiz_1 drv_allow_drive: if inp_joy_1 > DRV_AXIS_1_LOWER_BUFFER and inp_joy_1 < DRV_AXIS_1_UPPER_BUFFER then drv_axis_2_calib '-------------axis 1 calibration-------------- if inp_joy_1 > DRV_CALIB_1_REST then drv_axis_1_calib_higher inp_joy_1 = ((2000 + DRV_JOY_CENTER + ((inp_joy_1 - DRV_AXIS_1_UPPER_BUFFER) * DRV_JOY_MAX_RANGE / DRV_CALIB_1_UPPER_RANGE))min 2000 max 2127) - 2000 goto drv_axis_2_calib drv_axis_1_calib_higher: inp_joy_1 = ((2000 + DRV_JOY_CENTER - ((DRV_AXIS_1_LOWER_BUFFER - inp_joy_1) * DRV_JOY_MIN_RANGE / DRV_CALIB_1_LOWER_RANGE))min 2127 max 2255) - 2000 'intentionally falls through '---------------axis 2 calibration-------------- '------------determine y axis or x axis-------- drv_axis_2_calib: if drv_joystick_switch = DRV_ONE_JOYSTICK then drv_axis_2_x_calib '------calibrate axis 2 as axis y of 2 joystick if inp_joy_2 < DRV_AXIS_2_Y_LOWER_BUFFER or inp_joy_2 > DRV_AXIS_2_Y_UPPER_BUFFER then drv_axis_2_y_calib_det inp_joy_2 = DRV_JOY_CENTER goto drv_fine_cont drv_axis_2_y_calib_det: if inp_joy_2 > DRV_CALIB_REST_Y_2 then drv_axis_2_y_calib_above inp_joy_2 = ((2000 + DRV_JOY_CENTER - ((DRV_AXIS_2_Y_LOWER_BUFFER - inp_joy_2) * DRV_JOY_MIN_RANGE / DRV_CALIB_2_Y_LOWER_RANGE))min 2000 max 2127) - 2000 goto drv_fine_cont drv_axis_2_y_calib_above: inp_joy_2 = ((2000 + DRV_JOY_CENTER + ((inp_joy_2 - DRV_AXIS_2_Y_UPPER_BUFFER) * DRV_JOY_MAX_RANGE / DRV_CALIB_2_Y_UPPER_RANGE))min 2127 max 2255) - 2000 goto drv_fine_cont '------calibrate axis 2 as x axis of 1 joystick---- drv_axis_2_x_calib: if inp_joy_2 < DRV_AXIS_2_Y_LOWER_BUFFER or inp_joy_2 > DRV_AXIS_2_Y_UPPER_BUFFER then drv_axis_2_x_calib_det inp_joy_2 = DRV_JOY_CENTER goto drv_fine_cont drv_axis_2_x_calib_det: if inp_joy_2 > DRV_CALIB_REST_X_2 then drv_axis_2_x_calib_above inp_joy_2 = ((2000 + DRV_JOY_CENTER - ((DRV_AXIS_2_X_RIGHT_BUFFER - inp_joy_2) * DRV_JOY_MIN_RANGE / DRV_CALIB_2_X_RIGHT_RANGE))min 2000 max 2127) - 2000 goto drv_fine_cont drv_axis_2_x_calib_above: inp_joy_2 = ((2000 + DRV_JOY_CENTER + ((inp_joy_2 - DRV_AXIS_2_X_LEFT_BUFFER) * DRV_JOY_MAX_RANGE / DRV_CALIB_2_X_LEFT_RANGE))min 2127 max 2255) - 2000 'intentionally falls through 'after calibrates goes to determine if finecontrol switch is on 'if yes then sets motors to 80%, if not then goes set drive output drv_fine_cont: if inp_drv_fc = DRV_FINE_ON then drv_fine_cont_set goto drv_drive_output_set drv_fine_cont_set: inp_joy_1 = ((2000 + (((DRV_FINE_CON_DIV - DRV_FINE_CON_MUL) * DRV_JOY_CENTER) + (DRV_FINE_CON_MUL * inp_joy_1)) / DRV_FINE_CON_DIV)min 2000 max 2255) - 2000 inp_joy_2 = ((2000 + (((DRV_FINE_CON_DIV - DRV_FINE_CON_MUL) * DRV_JOY_CENTER) + (DRV_FINE_CON_MUL * inp_joy_2)) / DRV_FINE_CON_DIV)min 2000 max 2255) - 2000 'intentionally falls through '---------set drive outputs-------- drv_drive_output_set: if drv_joystick_switch = DRV_ONE_JOYSTICK then drv_one_joy_set out_drive_right = inp_joy_1 out_drive_left = inp_joy_2 goto drv_turbo_det 'sets tic tac to board drv_one_joy_set: out_drive_left = ((2000 + inp_joy_1 - inp_joy_2 + DRV_MOTOR_STOP)min 2000 max 2255) - 2000 out_drive_right = ((2000 + inp_joy_1 + inp_joy_2 - DRV_MOTOR_STOP)min 2000 max 2255) - 2000 'intentionally falls through '----------set turbo output---------- 'determines if turbo switch is on, if yes then goes to reverse motors drv_turbo_det: if inp_turbo > DRV_TURBO_OFF then drv_drive_r_reverse out_drive_left = ((2000 + (((DRV_SCAL_DIV - DRV_SCAL_MUL) * DRV_JOY_CENTER) + (DRV_SCAL_MUL * out_drive_left)) / DRV_SCAL_DIV)min 2000 max 2255) - 2000 out_drive_right = ((2000 + (((DRV_SCAL_DIV - DRV_SCAL_MUL) * DRV_JOY_CENTER) + (DRV_SCAL_MUL * out_drive_right)) / DRV_SCAL_DIV)min 2000 max 2255) - 2000 'intentionally falls through '---------reverse right motor------------ 'reverses right motor, so does direction actually wanted to go in drv_drive_r_reverse: out_drive_right = ((2000 + (2 * DRV_MOTOR_STOP) - out_drive_right)min 2000 max 2255) - 2000 return 'return to state master