'actual file is .bsx not .txt '{$STAMP BS2sx} '{$PBASIC 2.5} 'p1_x VAR byte 'Port 1, X-axis on Joystick 'p2_x VAR byte 'Port 2, X-axis on Joystick 'p3_x VAR byte 'Port 3, X-axis on Joystick 'p4_x VAR byte 'Port 4, X-axis on Joystick p1_y VAR byte 'Port 1, Y-axis on Joystick 'p2_y VAR byte 'Port 2, Y-axis on Joystick p3_y VAR byte 'Port 3, Y-axis on Joystick 'p4_y VAR byte 'Port 4, Y-axis on Joystick 'p1_wheel VAR byte 'Port 1, Wheel on Joystick 'p2_wheel VAR byte 'Port 2, Wheel on Joystick 'p3_wheel VAR byte 'Port 3, Wheel on Joystick 'p4_wheel VAR byte 'Port 4, Wheel on Joystick 'p1_aux VAR byte 'Port 1, Aux on Joystick 'p2_aux VAR byte 'Port 2, Aux on Joystick 'p3_aux VAR byte 'Port 3, Aux on Joystick 'p4_aux VAR byte 'Port 4, Aux on Joystick '---------- Operator Interface - Digital Inputs ------------------------------------------------------ oi_swA VAR byte 'OI Digital Switch Inputs 1 thru 8 oi_swB VAR byte 'OI Digital Switch Inputs 9 thru 16 '---------- Robot Controller (RC) - Analog Inputs ---------------------------------------------------- sensor1 VAR byte 'RC Analog Input 1, connector pin 2 'sensor2 VAR byte 'RC Analog Input 2, connector pin 16 sensor3 VAR byte 'RC Analog Input 3, connector pin 5 'sensor4 VAR byte 'RC Analog Input 4, connector pin 19 'sensor5 VAR byte 'RC Analog Input 5, connector pin 8 'sensor6 VAR byte 'RC Analog Input 6, connector pin 22 'sensor7 VAR byte 'RC Analog Input 7, connector pin 11 bat_volt VAR byte 'RC Analog Input 8, hardwired to the Battery 'Vin = ((4.7/14.7)* Battery voltage)-0.4 'Binary Battery Voltage = (Vin/5.0 V)*255 '---------- Robot Controller - Digital Inputs -------------------------------------------------------- rc_swA VAR byte 'RC Digital Inputs 1 thru 8 rc_swB VAR byte 'RC Digital Inputs 9 thru 16 '---------- Robot Controller - Digital Outputs ------------------------------------------------------- relayA VAR byte relayB VAR byte '---------- Misc. ------------------------------------------------------------------------------------ PB_mode VAR byte 'packet_num VAR byte delta_t VAR byte '========== DEFINE CONSTANTS (DO NOT CHANGE) ========================================================= '===================================================================================================== ' Baud rate for communications with User CPU OUTBAUD CON 20 ' (62500, 8N1, Noninverted) INBAUD CON 20 ' (62500, 8N1, Noninverted) USERCPU CON 4 FPIN CON 1 COMA CON 1 COMB CON 2 COMC CON 3 '===================================================================================================== '========== DEFINE ALIASES =========================================================================== '===================================================================================================== ' Aliases don't require any additional RAM. '---------- Aliases for each OI switch input --------------------------------------------------------- ' Below are aliases for the digital inputs located on the Operator Interface. ' Ports 1 & 3 have their inputs duplicated in ports 4 & 2 respectively. The ' inputs from ports 1 & 3 may be disabled via the 'Disable' dip switch ' located on the Operator Interface. See Users Manual for details. p1_sw_trig VAR oi_swA.bit0 'Joystick Trigger Button, same as Port4 pin5 p1_sw_top VAR oi_swA.bit1 'Joystick Top Button, same as Port4 pin8 p1_sw_aux1 VAR oi_swA.bit2 'Aux input, same as Port4 pin9 p1_sw_aux2 VAR oi_swA.bit3 'Aux input, same as Port4 pin15 p3_sw_trig VAR oi_swA.bit4 'Joystick Trigger Button, same as Port2 pin5 p3_sw_top VAR oi_swA.bit5 'Joystick Top Button, same as Port2 pin8 p3_sw_aux1 VAR oi_swA.bit6 'Aux input, same as Port2 pin9 p3_sw_aux2 VAR oi_swA.bit7 'Aux input, same as Port2 pin15 p2_sw_trig VAR oi_swB.bit0 'Joystick Trigger Button p2_sw_top VAR oi_swB.bit1 'Joystick Top Button p2_sw_aux1 VAR oi_swB.bit2 'Aux input p2_sw_aux2 VAR oi_swB.bit3 'Aux input p4_sw_trig VAR oi_swB.bit4 'Joystick Trigger Button p4_sw_top VAR oi_swB.bit5 'Joystick Top Button p4_sw_aux1 VAR oi_swB.bit6 'Aux input p4_sw_aux2 VAR oi_swB.bit7 'Aux input '---------- Aliases for each RC switch input --------------------------------------------------------- ' Below are aliases for the digital inputs located on the Robot Controller. rc_sw1 VAR rc_swA.bit0 rc_sw2 VAR rc_swA.bit1 rc_sw3 VAR rc_swA.bit2 rc_sw4 VAR rc_swA.bit3 rc_sw5 VAR rc_swA.bit4 rc_sw6 VAR rc_swA.bit5 rc_sw7 VAR rc_swA.bit6 rc_sw8 VAR rc_swA.bit7 rc_sw9 VAR rc_swB.bit0 rc_sw10 VAR rc_swB.bit1 rc_sw11 VAR rc_swB.bit2 rc_sw12 VAR rc_swB.bit3 rc_sw13 VAR rc_swB.bit4 rc_sw14 VAR rc_swB.bit5 rc_sw15 VAR rc_swB.bit6 rc_sw16 VAR rc_swB.bit7 '---------- Aliases for each RC Relay outputs -------------------------------------------------------- ' Below are aliases for the relay outputs located on the Robot Controller. ' RC Relays Port A relay1_fwd VAR RelayA.bit0 relay1_rev VAR RelayA.bit1 relay2_fwd VAR RelayA.bit2 relay2_rev VAR RelayA.bit3 relay3_fwd VAR RelayA.bit4 relay3_rev VAR RelayA.bit5 relay4_fwd VAR RelayA.bit6 relay4_rev VAR RelayA.bit7 ' RC Relays Port B relay5_fwd VAR RelayB.bit0 relay5_rev VAR RelayB.bit1 relay6_fwd VAR RelayB.bit2 relay6_rev VAR RelayB.bit3 relay7_fwd VAR RelayB.bit4 relay7_rev VAR RelayB.bit5 relay8_fwd VAR RelayB.bit6 relay8_rev VAR RelayB.bit7 '---------- Aliases for the Pbasic Mode Byte (PB_mode) ----------------------------------------------- ' Bit 7 of the PB_mode byte (aliased as comp_mode below) indicates the status ' of the Competition Control, either Enabled or Disabled. This indicates the ' starting and stopping of rounds at the competitions. ' Comp_mode is indicated by a solid "Disabled" LED on the Operator Interface. ' Comp_mode = 1 for Enabled, 0 for Disabled. ' ' Bit 6 of the PB_mode byte (aliased as auton_mode below) indicates the status ' of the Autonomous Mode, either Autonomous or Normal. This indicates when ' the robot must run on its own programming. When in Autonomous Mode, all ' OI analog inputs are set to 127 and all OI switch inputs are set to 0 (zero). ' Auton_mode is indicated by a blinking "Disabled" LED on the Operator Interface. ' Auton_mode = 1 for Autonomous, 0 for Normal. ' ' Autonomous Mode can be turned ON by setting the RC to Team 0 (zero). ' ' Bit 5 of the PB_mode byte (aliased as user_display_mode below) indicates when ' the user selects the "User Mode" on the OI. PB_mode.bit5 is set to 1 in "User Mode". ' When the user selects channel, team number, or voltage, PB_mode.bit5 is set to 0 ' When in "User Mode", the eight Robot Feedback LED are turned kbOFF. ' Note: "User Mode" is identified by the letter u in the left digit (for 4 digit OI's) ' Note: "User Mode" is identified by decimal places on the right two digits (for 3 digit OI's) comp_mode VAR PB_mode.bit7 auton_mode VAR PB_mode.bit6 user_display_mode VAR PB_mode.bit5 '/* ' OUR VARIABLES BELOW ' */ '---------- Operator Interface (OI) - Analog Inputs -------------------------------------------------- Right_Hand_Drive VAR byte Left_Hand_Drive VAR byte Initial_Right VAR p1_y ' // Used by low-pass digital filter Initial_Left VAR p3_y '---------- Robot Controller (RC) - Analog Inputs ---------------------------------------------------- A_Control VAR sensor1 ' Potentiometer for Automous mode '---------- Aliases for each OI switch input --------------------------------------------------------- ' [Misc] Arm VAR p4_sw_trig Compressor VAR p3_sw_aux2 Mesh_Mode VAR p4_sw_aux1 Auto_Override VAR p4_sw_trig ' [Driver Control] Orientation VAR p1_sw_trig Light_Manual VAR p3_sw_trig OneStickR VAR p1_sw_top OneStickL VAR p3_sw_top ' [Shuttle Control] ' > Rotation Shuttle_Rotation VAR p3_sw_aux1 ' XXX need comments ' > Vertical control Shuttle_Up VAR p2_sw_trig ' XXX need comments Shuttle_Down VAR p2_sw_top ' [Strap Control] Strap_Up VAR p2_sw_aux1 ' XXX comments Strap_Down VAR p2_sw_aux2 '---------- Aliases for each RC switch input --------------------------------------------------------- 'Bin_Sensor VAR rc_sw1 ' XXX gah more comments.... too lazy right now Stack_Finder_LHS VAR rc_sw2 Stack_Finder_RHS VAR rc_sw5 LHS_Shuttle_Top_Limit VAR rc_sw6 LHS_Shuttle_Bottom_Limit VAR rc_sw9 RHS_Shuttle_Top_Limit VAR rc_sw10 RHS_Shuttle_Bottom_Limit VAR rc_sw13 Pressure_Switch VAR rc_sw14 '---------- Filter Variables ------------------------------------------------------------------------- Left_Hand_Drivefilt VAR Left_Hand_Drive Right_Hand_Drivefilt VAR Right_Hand_Drive bIsDeadLeft VAR bit bIsDeadRight VAR bit bIsDead VAR bit '---------- IPD Code Variables ----------------------------------------------------------------------- 'IPD_timer VAR byte '---------- Driver Code Variables -------------------------------------------------------------------- ' [Misc] ' status1 VAR byte ' [PWMs] ' > Actual variables 'PWM3 VAR byte 'PWM4 VAR byte 'PWM5 VAR byte 'PWM6 VAR byte 'PWM8 VAR byte ' > Aliases 'LHS_Shuttle VAR PWM3 'RHS_Shuttle VAR PWM4 'LHS_Strap VAR PWM5 'RHS_Strap VAR PWM6 'Light_Servo VAR PWM8 '---------- My Happy Variables --------------------------------------------------------------------- left_mot VAR Left_Hand_Drive 'left motor speed WILL NEED TO SYNC WITH PWM right_mot VAR Right_Hand_Drive 'right motor speed left_target VAR byte 'left motor target speed right_target VAR byte 'right motor target speed gyro_in VAR byte 'reading off gyro gyro_ave VAR byte 'running ave sensor_in VAR p1_sw_trig '1 for bin in view, 0 for none timer VAR word 'master timer counts cycles first_seek VAR bit 'first time, no collision problems searching VAR bit 'for indicating active search search_dir VAR bit '0 for left, 1 for right reaq VAR bit 'should be going straight, but no sensor reading '**** CONSTANTS '--timeing constants----------------------------------------------------------------- initial_forward CON 60 'time to go forward out of starting position inital_ram CON 100 'time to go fast forward on first knock run search_time CON 50 'time to search one way before switching to other cont_fwd CON 40 'time to continue forward after tracking lost '--other constants------------------------------------------------------------------- acc_by CON 5 'number of notches to accelerate each time FULLSPEED CON 254 'maximum pwm output, max speed for automode NOSPEED CON 127 'robot not moving SLOW_FORWARD CON 140 'slow forward speed gyro_center CON 125 'center of gyro reading gyro_sample CON 4 'number of samples to keep in running average turn_speed CON 135 'speed to turn when searching adjust_speed CON 3 'speed to bias when reaq'ing adjust_d CON 4 'devide by to get "advanced" adjust speed '-- start not moving --------------------------------------------------------- left_mot = NOSPEED right_mot = NOSPEED left_target = NOSPEED right_target = NOSPEED first_seek = 1 'we are in starting pos searching = 1 'we are looking for stack search_dir = 0 'start looking left reaq = 0 'we are tracking, not re_aq timer = 0 'duh '--- START MAIN LOOP ----------------------------------------------------------------- Loops: 'debug "loopsie ---- " 'debug dec timer,cr gosub sub_serin '//read input gosub sub_updateGyro if(Auto_Override = 0 AND auton_mode <> 1) then '//check exit conditions run 1 endif if(searching = 1) then '//start search routine if(first_seek = 1) then if(timer < initial_forward) then gosub drive_forward else timer = 0 first_seek = 0 gosub search_bins endif else gosub search_bins endif else if(sensor_in = 1) then gosub drive_forward_slow timer = 0 else gosub sub_reaq endif endif gosub accelerate '//update motor 'gosub sub_reverse_right gosub sub_fix gosub sub_serout timer = timer + 1 goto Loops sub_fix: if Pressure_Switch = 1 then relay1_fwd = 1 relay1_rev = 0 else relay1_fwd = 0 relay1_rev = 0 endif relay5_fwd = 1 relay5_rev = 0 return sub_reaq: if(timer < cont_fwd) then if(gyro_ave > gyro_center) then left_target = SLOW_FORWARD + ((gyro_ave - gyro_center) / adjust_d) right_target = SLOW_FORWARD endif if(gyro_ave < gyro_center) then right_target = SLOW_FORWARD + ((gyro_center - gyro_ave) / adjust_d) left_target = SLOW_FORWARD endif if(gyro_ave = gyro_center) then gosub drive_forward_slow endif else searching = 1 timer = 0 endif return sub_updateGyro: gyro_ave = ((gyro_ave * gyro_sample) + gyro_in) / (gyro_sample + 1) return search_bins: if(sensor_in = 1) then '//OH MY WE HAVE CONTACT! searching = 0 gosub drive_forward_slow else if(search_dir = 1) then right_target = turn_speed left_target = NOSPEED endif if(search_dir = 0) then right_target = NOSPEED left_target = turn_speed endif if(timer > search_time) then timer = 0 gosub switch_dir endif endif return switch_dir: if(search_dir = 0) then search_dir = 1 else search_dir = 0 endif return sub_halt: right_target = NOSPEED left_target = NOSPEED return sub_reverse_right: ' // Reverses the PWM for the right joystick (if needed) ' { 'if bIsDeadRight = FALSE then if Initial_Right > 127 then Right_Hand_Drive = Right_Hand_Drive - 127 Right_Hand_Drive = 127 - Right_Hand_Drive else Right_Hand_Drive = 127 - Right_Hand_Drive Right_Hand_Drive = 127 + Right_Hand_Drive endif 'endif ' } return drive_forward_slow: right_target = SLOW_FORWARD left_target = SLOW_FORWARD return drive_forward: '************************************************************************ THIS ONE 'debug "Driving Forward!",cr 'gyro_total = gyro_total + gyro_in 'gyro_n = gyro_n + 1 'if(gyro_n = gyro_sample) then ' buffer = gyro_total - (gyro_center * gyro_n) ' if(buffer > gyro_center) then ' left_target = FULLSPEED ' right_target = FULLSPEED - (buffer - gyro_center) ' endif ' if(buffer < gyro_center) then ' right_target = FULLSPEED ' left_target = FULLSPEED - (buffer - gyro_center) ' endif ' if(buffer = gyro_center) then ' left_target = FULLSPEED ' right_target = FULLSPEED ' endif ' 'reset gyro vars ' gyro_n = gyro_n + 1 ' gyro_total = gyro_total / gyro_n 'reset average for n=1 ' endif right_target = FULLSPEED left_target = FULLSPEED return '************************************** '**** changes motor speed by acc_by to be closer to target speed accelerate: if(left_mot > left_target) then if(left_mot - left_target > acc_by) then left_mot = left_mot - acc_by endif if(left_mot - left_target <= acc_by) then left_mot = left_mot - 1 endif endif if(left_mot < left_target) then if(left_target - left_mot > acc_by) then left_mot = left_mot + acc_by endif if(left_target - left_mot <= acc_by) then left_mot = left_mot + 1 endif endif if(right_mot > right_target) then if(right_mot - right_target > acc_by) then right_mot = right_mot - acc_by endif if(right_mot - right_target <= acc_by) then right_mot = right_mot - 1 endif endif if(right_mot < right_target) then if(right_target - right_mot > acc_by) then right_mot = right_mot + acc_by endif if(right_target - right_mot <= acc_by) then right_mot = right_mot + 1 endif endif return return sub_serin: ' { Serin COMA\COMB, INBAUD, [oi_swA,oi_swB,rc_swA,rc_swB,PB_mode,sensor1,p1_y,sensor3,p3_y,delta_t] ' } return sub_serout: ' { Serout USERCPU, OUTBAUD, [255,255,Left_Hand_Drive,relayA,Right_Hand_Drive,relayB,127,127,127,127,127,127,127,127,127,127,127,127,127, 127] ' } return