You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
60 lines
2.1 KiB
60 lines
2.1 KiB
-- cause a rover to drive in a figure of eight pattern by directly controlling steering and throttle |
|
-- |
|
-- CAUTION: This script only works for Rover |
|
-- this script waits for the vehicle to be armed and RC6 input > 1800 and then: |
|
-- a) switches to Guided mode |
|
-- b) increases throttle to 30% and turns right at 20% for 10 seconds |
|
-- c) keep throttle at 30% and turns left at 20% for 10 seconds |
|
-- d) switches to Hold mode |
|
|
|
local stage = 0 |
|
local stage_counter = 0 |
|
local rover_guided_mode_num = 15 |
|
local rover_hold_mode_num = 4 |
|
|
|
-- the main update function that directly sets throttle and steering out to drive a figure of eight pattern |
|
function update() |
|
if not arming:is_armed() then -- reset state when disarmed |
|
stage = 0 |
|
stage_counter = 0 |
|
else |
|
pwm6 = rc:get_pwm(6) |
|
if pwm6 and pwm6 > 1800 then -- check if RC6 input has moved high |
|
if (stage == 0) then -- change to guided mode |
|
if (vehicle:set_mode(rover_guided_mode_num)) then -- change to Guided mode |
|
stage = stage + 1 |
|
stage_counter = 0 |
|
end |
|
elseif (stage == 1) then -- Stage1: increase throttle to 30% and turn right 20% |
|
if (vehicle:set_steering_and_throttle(0.2, 0.3)) then |
|
stage_counter = stage_counter + 1 |
|
if (stage_counter >= 10) then |
|
stage = stage + 1 |
|
stage_counter = 0 |
|
end |
|
end |
|
elseif (stage == 2) then -- Stage2: keep throttle at 30% and turn left 20% |
|
if (vehicle:set_steering_and_throttle(-0.2, 0.3)) then |
|
stage_counter = stage_counter + 1 |
|
if (stage_counter >= 10) then |
|
stage = stage + 1 |
|
stage_counter = 0 |
|
end |
|
end |
|
elseif (stage == 3) then -- Stage3: change to Hold mode |
|
vehicle:set_mode(rover_hold_mode_num) |
|
stage = stage + 1 |
|
gcs:send_text(0, "finished, switching to Hold") |
|
end |
|
else -- RC6 has been moved low |
|
if stage > 3 then |
|
stage = 0 |
|
stage_counter = 0 |
|
end |
|
end |
|
end |
|
|
|
return update, 1000 |
|
end |
|
|
|
return update()
|
|
|