rc.arm()
print(rc.getDeg())
# global "origin" parameter
START = 67
DEPTH_SEC = 3
SEC_PER_METER = 1
# depth hold and go down
rc.setmode('ALT_HOLD')
rc.throttle("time", DEPTH_SEC, -0.25)
# align with the origin
rc.imu_turn(START)
rc.forward("time", 6, 0.35)
while (ac.get_distance_fwd()[0] > 7000):
rc.forward("time", 1, 0.35)
rc.imu_turn(START - 45)
rc.forward("time", 5, 0.25)
rc.imu_turn(START + 45)
rc.forward("time", 5, 0.25)
rc.imu_turn(START + 135)
rc.forward("time", 5, 0.25)
rc.imu_turn(START + 225)
rc.forward("time", 5, 0.25)
counter = counter + 1
print 'wall', wallDist
print 'counter ', counter
diff = wallDist - value
print 'Laterl diff ', diff
rc.lateralDist(diff)
# depth hold and go down
rc.setmode('ALT_HOLD')
rc.throttle("time", DEPTH_SEC, -0.25)
# align with the origin
rc.imu_turn(START)
rc.forward("time", 10, 0.35)
leftAlign(2400)
rc.imu_turn(START)
rc.forward("time", 10, 0.35)
leftAlign(1500)
rc.imu_turn(START)
rc.forward("time", 13, 0.35)
leftAlign(3600)
rc.imu_turn(START + 182)
rc.forward("time", 13, 0.25)
rc.imu_turn(START + 182)
rightAlign(2300)
rc.forward("time", 13, 0.35)
rightAlign(2300)
rc.imu_turn(START + 182)
counter = counter + 1
print 'wall', wallDist
print 'counter ', counter
diff = wallDist - value
print 'Laterl diff ', diff
rc.lateralDist(diff)
# depth hold and go down
rc.setmode('ALT_HOLD')
rc.throttle("time", DEPTH_SEC, -0.25)
# align with the origin
rc.imu_turn(START)
rc.forward("time", 10, 0.35)
leftAlign(2400)
rc.imu_turn(START)
rc.forward("time", 5, 0.35)
leftAlign(2000)
rc.imu_turn(START)
rc.forward("time", 5, 0.35)
leftAlign(1500)
rc.imu_turn(START)
rc.forward("time", 5, 0.35)
leftAlign(1500)
rc.forward("time", 4, 0.35)
leftAlign(2500)
leftAlign(3500)
rc.imu_turn(70)
wallDist = 500
print 'wall', wallDist
print 'counter ', counter
diff = wallDist - value
print 'Laterl diff ', diff
rc.lateralDist(diff)
# depth hold and go down
rc.setmode('ALT_HOLD')
rc.throttle("time", DEPTH_SEC, -0.25)
# align with the origin
rc.imu_turn(START)
rc.forward("time", 10, 0.32)
leftAlign(2400)
rc.imu_turn(START)
rc.forward("time", 5, 0.32)
leftAlign(2000)
rc.imu_turn(START)
rc.forward("time", 5, 0.32)
leftAlign(1500)
rc.imu_turn(START)
rc.forward("time", 5, 0.32)
leftAlign(1500)
rc.forward("time", 5, 0.32)
leftAlign(3500)
rc.imu_turn(85)
rc.forward("time", 15, 0.25)
print(rc.getDeg())
#time.sleep(2)
#rc.forward("time", 6, 0.25)
#rc.yaw("time", 1.2, -0.25)
#rc.forward("time", 3, 0.25)
#rc.yaw("time", 1.2, -0.25)
#rc.forward("time", 6, 0.25)`
start = 73 + 180
rc.setmode('ALT_HOLD')
rc.throttle("time", 3, -0.25)
rc.imu_turn(start)
rc.forward("time", 5, 0.4)
rc.imu_turn(start - 45)
rc.forward("time", 1.5, 0.25)
rc.imu_turn(start + 45)
rc.forward("time", 1.5, 0.25)
rc.imu_turn(start + 135)
rc.forward("time", 1.5, 0.25)
rc.imu_turn(start + 180)
rc.forward("time", 5, 0.5)
#rc.imu_turn(121.5)
rc = RCLib()
ac = ACLib()
rc.setmode('ALT_HOLD')
rc.arm()
rc.throttle("time", 3, -0.20)
#i = 0
#while i<500:
# ac.get_distance_right()
# rc.setmode('ALT_HOLD')
# time.sleep(0.1)
# i = i + 1
rc.forward("time", 6, 0.1)
wallDist = 0
counter = 0
while ((ac.get_distance_right()[1] < 70) and (counter < 50)):
wallDist = ac.get_distance_right()[0]
counter = counter +1
print 'wall' , wallDist
print 'counter ' , counter
diff = ac.get_distance_right()[0] - 1500
print 'Laterl diff ' , diff
rc.lateralDist(diff)
rc.close()
from navigation.rc import RCLib
from navigation.log import *
import time
import navigation.imu as imu
log = LogLib()
log.setLevel(INFO)
rc = RCLib(log)
rc.setmode('ALT_HOLD')
rc.arm()
rc.throttle("time", 1, -0.25)
rc.forward("time", 5, 1)
log.flush()
current_val = dist
diff = abs(current_val - prev_val)
diff_accum = diff + diff_accum
rc.raw('yaw', pwm)
log.info(' prev %d' % prev_val)
log.info(' current %d' % current_val)
log.info(' diff %d' % diff)
log.info(' accu_diff %d' % diff_accum)
else:
rc.raw('yaw', pwm)
pass
time.sleep(0.01)
rc.raw('yaw', 1500)
print("Starting Buoy Test")
log.info('Starting buoy test')
rc.setmode('ALT_HOLD')
rc.throttle("time", 2, -0.25)
lowestSonar(0.16)
fwdSonar.stop()
buoyDist, _ = fwdSonar.dist()
buoyDist = buoyDist / 2000
log.info("buoyDist %d" % buoyDist)
rc.forward("time", 5, 0.32)
rc.close()
log.info('Done buoy test')
time.sleep(1)
log.flush()
print("Done Buoy Test")
# elif (steerinp < 0.15) :
# steer = 0.15
# return steer
#def fixAngle():
# error = rc.getDeg() - 48
#
# while (abs(error) > 3):
# steer = abs(error)*kP
# steer = clipSteer(steer)
#
# if (error>0) rc.raw('yaw', (1500 - (400*steer)) elif (error<0) rc.raw('yaw', (1500 + (400*steer)
# error = rc.getDeg() -48
originAngle = 48
rc = RCLib()
rc.setmode('ALT_HOLD')
rc.arm()
#fixAngle()
#endTime = time.time() + 7
#while(time.time() < endTime):
# rc.raw('pitch', 1700)
rc.throttle("time", 2.5, -0.5)
rc.forward("time", 8, 0.4)
print(rc.getDeg())
# global "origin" parameter
START = 240
DEPTH_SEC = 5
SEC_PER_METER = 1
GATECENTER = 2500
# depth hold and go down
rc.setmode('ALT_HOLD')
rc.throttle("time", DEPTH_SEC, -0.25)
# align with the origin
rc.imu_turn(START + 180)
rc.forward("time", 5, -0.35)
pingReturn = ac.get_distance_right()
wallDist = pingReturn[0]
confidence = pingReturn[1]
counter = 0
while ((confidence < 70) and (counter < 50)):
pingReturn = ac.get_distance_right()
wallDist = pingReturn[0]
confidence = pingReturn[1]
counter = counter + 1
print 'wall', wallDist
print 'counter ', counter
diff = wallDist - GATECENTER
print 'Laterl diff ', diff
rc.lateralDist(diff)
rc.imu_turn(START + 180)
print(rc.getDeg())
# global "origin" parameter
START = 240
DEPTH_SEC = 5
SEC_PER_METER = 1
GATECENTER = 2500
# depth hold and go down
rc.setmode('ALT_HOLD')
rc.throttle("time", DEPTH_SEC, -0.25)
# align with the origin
rc.imu_turn(START)
rc.forward("time", 15, 0.35)
while (ac.get_distance_fwd()[0] > 8200):
rc.forward("time", 1, 0.35)
rc.imu_turn(START - 45)
rc.forward("time", 5, 0.25)
rc.imu_turn(START + 45)
rc.forward("time", 5, 0.25)
rc.imu_turn(START + 135)
rc.forward("time", 5, 0.25)
rc.imu_turn(START + 225)
rc.forward("time", 5, 0.25)
