'=============================================================== ' PROGRAM: Hart Burn basic run with one joystick ' Written by: Ryan Waliany with code from Innovation First's ' default program (reorganized/restructured/redone) ' Date: Dec 24th 2002 ' Updated: Jan 5th 2003 '=============================================================== ' {$STAMP BS2SX} '=============================================================== '=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- ' Variables '=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- '++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ' Operator Interface - Analog Inputs, 0 to 254 '++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ '[ Port 1 ] p1_x VAR byte 'Port 1, X-axis on Joystick p1_y VAR byte 'Port 1, Y-axis on Joystick 'p1_wheel VAR byte 'Port 1, Wheel on Joystick 'p1_aux VAR byte 'Port 1, Auxilary Input ' on Joystick '[ Port 2 ] 'p2_x VAR byte 'Port 2, X-axis on Joystick 'p2_y VAR byte 'Port 2, Y-axis on Joystick 'p2_wheel VAR byte 'Port 2, Wheel on Joystick 'p2_aux VAR byte 'Port 2, Auxilary Input ' on Joystick '[ Port 3 ] 'p3_x VAR byte 'Port 3, X-axis on Joystick 'p3_y VAR byte 'Port 3, Y-axis on Joystick 'p3_wheel VAR byte 'Port 3, Wheel on Joystick 'p3_aux VAR byte 'Port 3, Auxilary Input ' on Joystick '[ Port 4 ] 'p4_x VAR byte 'Port 4, X-axis on Joystick 'p4_y VAR byte 'Port 4, Y-axis on Joystick 'p4_wheel VAR byte 'Port 4, Wheel on Joystick 'p4_aux VAR byte 'Port 4, Auxilary Input ' on Joystick '++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ' Operator Interface - Digital Inputs, buttons, 0-1, etc... '++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ oi_swi VAR WORD 'Operator Interface Digital 'Switch Inputs 1 to 16. '++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ' Robot Controller - Analog Inputs, 0 to 254 '++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ '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, buttons, 0-1, etc... '++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ rc_swi VAR WORD 'Robot Controller Digital 'Switch Inputs 1 to 16. '++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ' Robot Controller - Digital Outputs, buttons, 0-1, etc... '++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ relay VAR WORD 'Robot Controller Digital 'Switch Outputs 1 to 16. '++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ' Misc... '++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ PB_mode VAR byte 'packet_num VAR byte 'delta_t VAR byte PWM1 VAR byte PWM2 VAR byte state VAR nib countx var byte count2 var byte '=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- ' Aliases '=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- '++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ' Operator Interface - Switch Inputs '++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ '[ Port 1 ] p1_sw_trig VAR oi_swi.bit0 'Joystick Trigger Button, same as Port4 pin5 p1_sw_top VAR oi_swi.bit1 'Joystick Top Button, same as Port4 pin8 p1_sw_aux1 VAR oi_swi.bit2 'Aux input, same as Port4 pin9 p1_sw_aux2 VAR oi_swi.bit3 'Aux input, same as Port4 pin15 '[ Port 2 ] p3_sw_trig VAR oi_swi.bit4 'Joystick Trigger Button, same as Port2 pin5 p3_sw_top VAR oi_swi.bit5 'Joystick Top Button, same as Port2 pin8 p3_sw_aux1 VAR oi_swi.bit6 'Aux input, same as Port2 pin9 p3_sw_aux2 VAR oi_swi.bit7 'Aux input, same as Port2 pin15 '[ Port 3 ] p2_sw_trig VAR oi_swi.bit8 'Joystick Trigger Button p2_sw_top VAR oi_swi.bit9 'Joystick Top Button p2_sw_aux1 VAR oi_swi.bit10 'Aux input p2_sw_aux2 VAR oi_swi.bit11 'Aux input '[ Port 4 ] p4_sw_trig VAR oi_swi.bit12 'Joystick Trigger Button p4_sw_top VAR oi_swi.bit13 'Joystick Top Button p4_sw_aux1 VAR oi_swi.bit14 'Aux input p4_sw_aux2 VAR oi_swi.bit15 'Aux input '++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ' Robot Controller - Switch Inputs '++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ rc_sw1 VAR rc_swi.bit0 rc_sw2 VAR rc_swi.bit1 rc_sw3 VAR rc_swi.bit2 rc_sw4 VAR rc_swi.bit3 rc_sw5 VAR rc_swi.bit4 rc_sw6 VAR rc_swi.bit5 rc_sw7 VAR rc_swi.bit6 rc_sw8 VAR rc_swi.bit7 rc_sw9 VAR rc_swi.bit8 rc_sw10 VAR rc_swi.bit9 rc_sw11 VAR rc_swi.bit10 rc_sw12 VAR rc_swi.bit11 rc_sw13 VAR rc_swi.bit12 rc_sw14 VAR rc_swi.bit13 rc_sw15 VAR rc_swi.bit14 rc_sw16 VAR rc_swi.bit15 '++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ' Robot Controller - Relay Outputs '++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ relay1_fwd VAR Relay.bit0 relay1_rev VAR Relay.bit1 relay2_fwd VAR Relay.bit2 relay2_rev VAR Relay.bit3 relay3_fwd VAR Relay.bit4 relay3_rev VAR Relay.bit5 relay4_fwd VAR Relay.bit6 relay4_rev VAR Relay.bit7 relay5_fwd VAR Relay.bit8 relay5_rev VAR Relay.bit9 relay6_fwd VAR Relay.bit10 relay6_rev VAR Relay.bit11 relay7_fwd VAR Relay.bit12 relay7_rev VAR Relay.bit13 relay8_fwd VAR Relay.bit14 relay8_rev VAR Relay.bit15 '++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ' Misc. '++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ' 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 OFF. ' 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 '=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- ' Constants '=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- '++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ' Basic Run '++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ON con 1 OFF con 0 TRUE con 1 FALSE con 0 '++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ' Initialization '++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ c_p1_y CON ON c_p2_y CON OFF c_p3_y CON OFF c_p4_y CON OFF c_p1_x CON ON c_p2_x CON OFF c_p3_x CON OFF c_p4_x CON OFF c_p1_wheel CON OFF c_p2_wheel CON OFF c_p3_wheel CON OFF c_p4_wheel CON OFF c_p1_aux CON OFF c_p2_aux CON OFF c_p3_aux CON OFF c_p4_aux CON OFF c_oi_swi CON ON c_sensor1 CON OFF c_sensor2 CON OFF c_sensor3 CON ON c_sensor4 CON OFF c_sensor5 CON OFF c_sensor6 CON OFF c_sensor7 CON OFF c_batt_volt CON OFF c_rc_swi CON ON c_delta_t CON OFF c_PB_mode CON ON c_packet_num CON OFF c_res01 CON OFF '++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ' Critical Constants '++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ '::Formula: V = INT ((DESIRED FLASH VOLTAGE + 0.4) * 16.3) '::The Low-Battery light will flash if the voltage is below '::this set voltage at anytime during the program. LowBatteryVolt CON 120 ' 7 Volts 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 '=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- ' RoboLoop '=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- gosub sub_init RoboLoop: '{ '+++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ' Input '+++++++++++++++++++++++++++++++++++++++++++++++++++++++++ gosub sub_serin '+++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ' Manipulation '+++++++++++++++++++++++++++++++++++++++++++++++++++++++++ if auton_mode <> ON then skipcontrol gosub sub_automation skipcontrol: if auton_mode <> OFF then skipautomation gosub sub_setRelays gosub sub_tankDrive ' gosub sub_balance 'balances the robot uses sensor3 'eduRobot with yaw rate sensor + 2 servos skipautomation: '+++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ' Output '+++++++++++++++++++++++++++++++++++++++++++++++++++++++++ gosub sub_ledFeedback gosub sub_serout '} GoTo RoboLoop '=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- ' Sub Routines '=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- '++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ' Sub: sub_init ' Desc: Basic Initialization '++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ sub_init: gosub init_input ' Declaring the Input gosub init_output ' Declaring and Initializing the Output gosub init_neutral ' Initializing the analog inputs to 127 gosub init_master ' PBASIC OI initialization and synchronization return '++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ' Sub: init_input ' Desc: Initializes the basic inputs for initialization '++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ init_input: Input COMA Input COMC return '++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ' Sub: init_output ' Desc: Initializes the basic outputs of the RC and OI '++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ init_output: Output COMB Output 7 'define Basic Run LED on RC => out7 Output 8 'define Robot Feedback LED => out8 => PWM1 Green Output 9 'define Robot Feedback LED => out9 => PWM1 Red Output 10 'define Robot Feedback LED => out10 => PWM2 Green Output 11 'define Robot Feedback LED => out11 => PWM2 Red Output 12 'define Robot Feedback LED => out12 => Relay1 Red Output 13 'define Robot Feedback LED => out13 => Relay1 Green Output 14 'define Robot Feedback LED => out14 => Relay2 Red Output 15 'define Robot Feedback LED => out15 => Relay2 Green Out7 = ON 'Basic Run LED on RC Out8 = OFF 'PWM1 LED - Green Out9 = OFF 'PWM1 LED - Red Out10 = OFF 'PWM2 LED - Green Out11 = OFF 'PWM2 LED - Red Out12 = OFF 'Relay1 LED - Red Out13 = OFF 'Relay1 LED - Green Out14 = OFF 'Relay2 LED - Red Out15 = OFF 'Relay2 LED - Green return '++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ' Sub: init_neutral ' Desc: Initializes the analog inputs to a neutral position '++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ init_neutral: '[ Port 1 ] p1_x = 127 'Port 1, X-axis on Joystick p1_y = 127 'Port 1, Y-axis on Joystick 'p1_wheel = 127 'Port 1, Wheel on Joystick 'p1_aux = 127 'Port 1, Aux Analog '[ Port 2 ] 'p2_x = 127 'Port 2, X-axis on Joystick 'p2_y = 127 'Port 2, Y-axis on Joystick 'p2_wheel = 127 'Port 2, Wheel on Joystick 'p2_aux = 127 'Port 2, Aux Analog '[ Port 3 ] 'p3_x = 127 'Port 3, X-axis on Joystick 'p3_y = 127 'Port 3, Y-axis on Joystick 'p3_wheel = 127 'Port 3, Wheel on Joystick 'p3_aux = 127 'Port 3, Aux Analog '[ Port 4 ] 'p4_x = 127 'Port 4, X-axis on Joystick 'p4_y = 127 'Port 4, Y-axis on Joystick 'p4_wheel = 127 'Port 4, Wheel on Joystick 'p4_aux = 127 'Port 4, Aux Analog 'Misc sensor3 = 127 countx = 0 count2 = 0 state = 0 return '++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ' Sub: init_master ' Desc: Master initialization, synchronizes the Operator ' Interface with Serin. '++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ init_master: initByteA var byte initByteB var byte initByteC var byte initByteD var byte initByteA = c_p3_x << 1 + c_p4_x << 1 + c_p1_x << 1 + c_p2_x << 1 + c_rc_swi initByteA = initByteA << 1 + c_rc_swi << 1 + c_oi_swi << 1 + c_oi_swi initByteB = c_sensor4 << 1 + c_sensor3 << 1 + c_p1_y << 1 + c_p2_y << 1 + c_sensor2 initByteB = initByteB << 1 + c_sensor1 << 1 + c_packet_num << 1 + c_PB_mode initByteC = c_batt_volt << 1 + c_sensor7 << 1 + c_p1_wheel << 1 + c_p2_wheel << 1 + c_sensor6 initByteC = initByteC << 1 + c_sensor5 << 1 + c_p3_y << 1 + c_p4_y initByteD = c_res01 << 1 + c_delta_t << 1 + c_p3_aux << 1 + c_p4_aux << 1 + c_p1_aux initByteD = initByteD << 1 + c_p2_aux << 1 + c_p3_wheel << 1 + c_p4_wheel Output comA low comA low comB Wait_init: if IN3 = ON then Wait_init: Shiftout comB, comA, 1, [initByteA, initByteB, initByteC, initByteD,lowBatteryVolt] Input comA high comB return '++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ' Sub: sub_serin ' Desc: Receives input from the Operator Interface '++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ' Construct the "serin" command using the following rules: ' 1) There must be one variable for every input defined in ' the "Define Constants for Init" section. ' 2) The order must match the order in the EXAMPLE SERIN ' COMMAND below. ' 3) The total number of all variables may not exceed 26. ' 4) Only use one "Serin" command. ' 5) The Serin command must occupy one line. ' ' If you see a BASIC INIT ERR on the Robot Controller after ' programming and pressing RESET, then there is a problem ' with the Serin command below. Check the number of variables. ' A BASIC INIT ERR will not occur if you have the variables in ' the wrong order, however your code will not work correctly. ' ' EXAMPLE SERIN COMMAND ' This example exceed the 26 variable limit and is not on one line: ' ' Serin COMA\COMB, INBAUD, [oi_swi.lowbyte,oi_swi.highbyte, ' rc_swi.lowbyte,rc_swi.highbyte,p2_x,p1_x,p4_x,p3_x,PB_mode, ' packet_num,sensor1, sensor2,p2_y,p1_y,sensor3,sensor4,p4_y, ' p3_y,sensor5,sensor6,p2_wheel,p1_wheel,sensor7,sensor8,p4_wheel, ' p3_wheel,p2_aux,p1_aux,p4_aux,p3_aux,delta_t,res01] ' '++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ sub_serin: Serin COMA\COMB, INBAUD, [oi_swi.lowbyte,oi_swi.highbyte,rc_swi.lowbyte,rc_swi.highbyte,p1_x,PB_mode,p1_y, sensor3] return '++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ' Sub: sub_setRelays ' Desc: Sets the relays '++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ sub_setRelays: relay1_fwd = p1_sw_trig 'Port 1 Trigger = Relay 1 Forward relay1_rev = p1_sw_top 'Port 1 Thumb = Relay 1 Reverse return '++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ' Sub: sub_tankDrive ' Desc: Tank Drive '++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ' This section modified the output of PWM1, and PWM 2 for ' control from one joystick (Port 1). The Out8 thru Out11 ' lines control the Feedback LEDs. ' Y-axis controls speed. ' X-axis turns. ' PWM1 - Left motor. ' PWM2 - Right motor. '++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ sub_tankDrive: 'Using 2000 to avoid negative numbers PWM1 = (((2000 + p1_y - p1_x + 127) Min 2000 Max 2254) - 2000) PWM2 = (((2000 + p1_y + p1_x - 127) Min 2000 Max 2254) - 2000) return '++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ' Sub: sub_ledFeedback ' Desc: Turns on and off the LEDs '++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ sub_ledFeedback: Toggle 7 'Basic Run LED on the RC is toggled ON/OFF every loop. if user_display_mode <> ON then skipuserdisplay 'Converts to integer on display 0-254 'This shows speed... Out8 = p1_y.bit0 Out9 = p1_y.bit1 Out10 = p1_y.bit2 Out11 = p1_y.bit3 Out12 = p1_y.bit4 Out13 = p1_y.bit5 Out14 = p1_y.bit6 Out15 = p1_y.bit7 skipuserdisplay: if user_display_mode <> OFF then skipnormaldisplay Out8 = pwm1/216 'LED is ON when Victor883 full forward Out9 = ~(pwm1/56 max 1) 'LED is ON when Victor883 full reverse Out10 = pwm2/216 'LED is ON when Victor883 full forward Out11 = ~(pwm2/56 max 1) 'LED is ON when Victor883 full reverse Out13 = relay1_fwd 'LED is ON when Relay 1 is Forward Out12 = relay1_rev 'LED is ON when Relay 1 is Reverse skipnormaldisplay: return '++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ' Sub: automation ' Desc: Runs automated process during 15 seconds '++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ sub_automation: 'Runs automated process during 15 seconds of no Operator Interface return '++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ' Sub: sub_balance ' Desc: Balance '++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ sub_balance: if state <> 0 then skipstate0 PWM1 = 183 PWM2 = 183 if sensor3 < 180 then skipstatechange1 state = 1 skipstatechange1: skipstate0: if state <> 1 then skipstate1 PWM1 = 67 PWM2 = 67 count2 = count2 + 1 if count2 < 30 then skipstatechange2 state = 2 skipstatechange2: skipstate1: if state <> 2 then skipstate2 'IDLE PWM1 = 127 PWM2 = 127 skipstate2: return '++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ' Sub: sub_serout ' Desc: Sends output to the pins on the Robot Controller '++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ' The Serout line sends data to the Output uP. The Output uP ' passes this to each PWM 1-16 and Relay 1-8. The Output uP ' will not output data if there is no communication with the ' Operator Interface or if the Competition Mode is Disabled. ' Do not delete any elements from the Serout array. Set ' unused PWM outputs to 127. Set unused relay outputs to 0. ' ' Serout USERCPU, OUTBAUD, [255,255,(PWM1),relayA,(PWM2),relayB, ' (PWM3),(PWM4),(PWM5),(PWM6),(PWM7),(PWM8),(PWM9),(PWM10), ' (PWM11),(PWM12),(PWM13),(PWM14),(PWM15),(PWM16)] '++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ sub_serout: 'Calibrate pwm2 for a reversed motor pwm2 = 254-pwm2 'Servos on EDUrobot move faster in one direction if between 127-200 and 54-127 'only needed for Balance if state = 0 then skipservofix if pwm1 > 127 then skipsetx pwm1 = pwm1 + 10 skipsetx: if pwm2 > 127 then skipsetx2 pwm2 = pwm2 + 10 skipsetx2: skipservofix: Serout USERCPU, OUTBAUD, [255,255,pwm1,relay.lowbyte, pwm2,relay.highbyte,127,127,127,127,127,127,127,127,127,127,127,127,127,127] return '=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=- ' End '=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-