|
for those of you who only speak PBasic, here is our code for closed loop steering - it uses the one joystick code to control the motors. Yaw is the signal from the Gyro (BTW the gyro was upside down on our bot)
'---------- 1 Joystick Drive ---------------------------------------------------------
'********************Gyro yaw rate closed loop steering ***************************
'compairs gyro signal to p1_x steering command to close the steering loop
if p1_sw_top=0 then gyro_override 'when button is pressed then close loop
AD: 'line following code jumps into here
yaw_error = (((2127 + p1_x - yaw) Min 2000 Max 2254)-2000)
yaw_command =(((1873 + yaw_error + yaw_error) Min 2000 Max 2254) -2000)
goto gyro_end
gyro_override:
yaw_command=p1_x
gyro_end: '************************************************* ***********************
' This code mixes the Y and X axis on Port 1 to allow one joystick drive.
' Joystick forward = Robot forward
' Joystick backward = Robot reverse
' Joystick right = Robot rotates right
' Joystick left = Robot rotates left
' Connect the left drive motors to PWM15 and/or PWM16
' Connect the right drive motors to PWM13 and/or PWM14
'NOTE: p1_x = 0 at full right, 255 at full left
drive_R = (((1873 + p1_y + yaw_command) Min 2000 Max 2254) - 2000)
drive_L = (((2127 + p1_y - yaw_command) Min 2000 Max 2254) - 2000)
|