def drive(sp): if True: if sp != 0 and not g.braking: g.speedsign = sign(sp) g.outspeedcm = sp*2 #print("outspeedcm = %d" % g.outspeedcm) else: # do this in readspeed2 instead # maybe, but then steer will zero the speed g.outspeed = sp if abs(sp) >= 7: g.speedtime = time.time() else: g.speedtime = None if sp != 0 and not g.braking: g.speedsign = sign(sp) if sp < 0: sp += 256 st = g.steering if st < 0: st += 256 tolog("motor %d steer %d" % (sp, g.steering)) cmd = "/home/pi/can-utils/cansend can0 '101#%02x%02x'" % ( sp, st) #print (sp, g.steering, cmd) os.system(cmd)
def drive(sp): if True: if sp != 0 and not g.braking: g.speedsign = sign(sp) g.outspeedcm = sp * 2 #print("outspeedcm = %d" % g.outspeedcm) else: # do this in readspeed2 instead # maybe, but then steer will zero the speed g.outspeed = sp if abs(sp) >= 7: g.speedtime = time.time() else: g.speedtime = None if sp != 0 and not g.braking: g.speedsign = sign(sp) if sp < 0: sp += 256 st = g.steering if st < 0: st += 256 tolog("motor %d steer %d" % (sp, g.steering)) cmd = "/home/pi/control-utils/cansend can0 '101#%02x%02x'" % (sp, st) #print (sp, g.steering, cmd) os.system(cmd)
def keepspeed(): outspeedi = 0 # 0 to 9 speeds = [0, 7, 11, 15, 19, 23, 27, 37, 41, 45, 49, # 93 to 100 haven't been run yet 53, 57, 73, 77, 81, 85, 89, 93, 97, 100] while True: time.sleep(0.1) if g.outspeedcm == None: continue spi = outspeedi desiredspeed = g.outspeedcm if g.limitspeed != None and desiredspeed > g.limitspeed: desiredspeed = g.limitspeed if g.user_pause: desiredspeed = 0 if desiredspeed > g.finspeed: if spi < len(speeds)-1: spi += 1 elif desiredspeed < g.finspeed: if spi > 0: spi -= 1 desiredspeed_sign = sign(desiredspeed) desiredspeed_abs = abs(desiredspeed) if True: # bypass the control spi = int(desiredspeed_abs/10) if spi > len(speeds)-1: spi = len(speeds)-1 sleeptime = 1 else: sleeptime = 3 sp = speeds[spi] outspeedi = spi # spi/outspeedi don't remember the sign sp *= desiredspeed_sign if abs(sp) >= 7: g.speedtime = time.time() else: g.speedtime = None if g.outspeed == sp and sp != 0: # pass continue if False: print("outspeedcm %f finspeed %f outspeedi %d spi %d sp %f outspeed %f" % ( g.outspeedcm, g.finspeed, outspeedi, spi, sp, g.outspeed)) g.outspeed = sp if sp != 0 and not g.braking: g.speedsign = sign(sp) if sp != 0: nav_signal.warningblink(False) # if sp < 0: # sp += 256 st = g.steering # if st < 0: # st += 256 tolog("motor %d steer %d" % (sp, st)) driving.dodrive(sp, st) time.sleep(sleeptime)
def keepspeed(): outspeedi = 0 # 0 to 9 speeds = [ 0, 7, 11, 15, 19, 23, 27, 37, 41, 45, 49, # 93 to 100 haven't been run yet 53, 57, 73, 77, 81, 85, 89, 93, 97, 100 ] while True: time.sleep(0.1) if g.outspeedcm == None: continue spi = outspeedi desiredspeed = g.outspeedcm if g.limitspeed != None and desiredspeed > g.limitspeed: desiredspeed = g.limitspeed if g.user_pause: desiredspeed = 0 if desiredspeed > g.finspeed: if spi < len(speeds) - 1: spi += 1 elif desiredspeed < g.finspeed: if spi > 0: spi -= 1 desiredspeed_sign = sign(desiredspeed) desiredspeed_abs = abs(desiredspeed) if True: # bypass the control spi = int(desiredspeed_abs / 10) if spi > len(speeds) - 1: spi = len(speeds) - 1 sleeptime = 1 else: sleeptime = 3 sp = speeds[spi] outspeedi = spi # spi/outspeedi don't remember the sign sp *= desiredspeed_sign if False: print( "outspeedcm %f finspeed %f outspeedi %d spi %d sp %f outspeed %f" % (g.outspeedcm, g.finspeed, outspeedi, spi, sp, g.outspeed)) if g.outspeed == sp and sp != 0: # pass continue g.outspeed = sp if abs(sp) >= 7: g.speedtime = time.time() else: g.speedtime = None if sp != 0 and not g.braking: g.speedsign = sign(sp) if sp != 0: nav_signal.warningblink(False) # if sp < 0: # sp += 256 st = g.steering # if st < 0: # st += 256 tolog("motor %d steer %d" % (sp, st)) driving.dodrive(sp, st) time.sleep(sleeptime)
def goto_1(x, y): g.targetx = x g.targety = y missed = False inc = 0 inc2 = 0 lastdist = None brake_s = 0.0 tolog("goto_1 %f %f" % (x, y)) while True: if g.remote_control: print("remote_control is true") return 2 if not checkpos(): if False: print("checkpos returned False") return 2 if g.poserror: if False: print("positioning system error") tolog("poserr 1") g.limitspeed = 0 time.sleep(10) tolog("poserr 2") g.limitspeed = None print("continuing") g.poserror = False tolog("poserr 3") return 2 if g.obstacle: print("obstacle") return 2 dist = getdist(x, y) if g.inspeed != 0: # Assume we are going in the direction of the target. # At low speeds, braking time is about 1.5 s. brake_s = 1.5 * abs(g.inspeed)/100 # say that braking distance is 1 dm at higher speed, when # braking electrically if g.inspeed > 0: brake_s = 0.4 else: brake_s = 0.6 # we should only use brake_s when we are going to stop brake_s = 0.0 # 'lastdist' is never non-None now, nor 'missed' if lastdist != None: if dist < lastdist - 0.01: inc = -1 lastdist = dist elif dist > lastdist + 0.01: if inc == -1: missed = True tolog("missed target") inc = 1 lastdist = dist tolog("gotoa1 %f %f -> %f %f" % (g.ppx, g.ppy, x, y)) a = atan2(y-g.ppy, x-g.ppx) adeg = 180/pi*a adeg = 90-adeg adiff = g.ang - adeg adiff = adiff%360 tolog("gotoa2 a %f adeg %f adiff %f" % (a, adeg, adiff)) if g.speedsign < 0: adiff += 180 if adiff > 180: adiff -= 360 adiff = -adiff # now, positive means the target is to the right of us tolog("gotoa3 adiff %f" % (adiff)) #print(adiff) # if dist < g.targetdist or dist < brake_s or missed: if (not g.allangles and abs(adiff) > 90) or dist < g.targetdist: if False: #stop("9") # driving.drive(-1) # continue a little so it can pass the target if it wasn't # there yet time.sleep(0.5) # driving.drive(-1) # time.sleep(0.2) driving.drive(0) #print("adiff %f dist %f" % (adiff, dist)) if dist < g.targetdist: #print("dist < %f" % g.targetdist) pass if abs(adiff) > 90: print("adiff = %f; leaving (%f,%f) behind" % (adiff,x,y)) return 1 return 0 asgn = sign(adiff) aval = abs(adiff) st = g.anglefactor*aval if st > 100: st = 100 st = asgn*g.speedsign*st if False: st_d = st - g.steering if st_d > 10: st = g.steering + 10 elif st_d < -10: st = g.steering - 10 driving.steer(st) tolog("gotoa4 steer %f" % (st)) if not g.simulate: nav_tc.send_to_ground_control("dpos %f %f %f %f 0 %f" % ( g.ppx,g.ppy,g.ang,time.time()-g.t0, g.finspeed)) tt0 = time.time() d = nav_map.roaddist(g.ppx, g.ppy) if d > g.slightlyoffroad: # print("roaddist %f at %f, %f" % (d, g.ppx, g.ppy)) if d > g.maxoffroad: print("roaddist %f at %f, %f" % (d, g.ppx, g.ppy)) return 2 period = 0.1*g.speedfactor tt1 = time.time() dtt = tt1-tt0 dtt1 = period - dtt if dtt1 > 0: time.sleep(period)
def goto_1(x, y): g.targetx = x g.targety = y missed = False inc = 0 inc2 = 0 lastdist = None brake_s = 0.0 tolog("goto_1 %f %f" % (x, y)) while True: if g.remote_control: print("remote_control is true") return 2 if not checkpos(): if False: print("checkpos returned False") return 2 if g.poserror: if False: print("positioning system error") tolog("poserr 1") g.limitspeed = 0 time.sleep(10) tolog("poserr 2") g.limitspeed = None print("continuing") g.poserror = False tolog("poserr 3") return 2 if g.obstacle: print("obstacle") return 2 dist = getdist(x, y) if g.inspeed != 0: # Assume we are going in the direction of the target. # At low speeds, braking time is about 1.5 s. brake_s = 1.5 * abs(g.inspeed) / 100 # say that braking distance is 1 dm at higher speed, when # braking electrically if g.inspeed > 0: brake_s = 0.4 else: brake_s = 0.6 # we should only use brake_s when we are going to stop brake_s = 0.0 # 'lastdist' is never non-None now, nor 'missed' if lastdist != None: if dist < lastdist - 0.01: inc = -1 lastdist = dist elif dist > lastdist + 0.01: if inc == -1: missed = True tolog("missed target") inc = 1 lastdist = dist tolog("gotoa1 %f %f -> %f %f" % (g.ppx, g.ppy, x, y)) a = atan2(y - g.ppy, x - g.ppx) adeg = 180 / pi * a adeg = 90 - adeg adiff = g.ang - adeg adiff = adiff % 360 tolog("gotoa2 a %f adeg %f adiff %f" % (a, adeg, adiff)) if g.speedsign < 0: adiff += 180 if adiff > 180: adiff -= 360 adiff = -adiff # now, positive means the target is to the right of us tolog("gotoa3 adiff %f" % (adiff)) #print(adiff) # if dist < g.targetdist or dist < brake_s or missed: if (not g.allangles and abs(adiff) > 90) or dist < g.targetdist: if False: #stop("9") # driving.drive(-1) # continue a little so it control pass the target if it wasn't # there yet time.sleep(0.5) # driving.drive(-1) # time.sleep(0.2) driving.drive(0) #print("adiff %f dist %f" % (adiff, dist)) if dist < g.targetdist: #print("dist < %f" % g.targetdist) pass if abs(adiff) > 90: print("adiff = %f; leaving (%f,%f) behind" % (adiff, x, y)) return 1 return 0 asgn = sign(adiff) aval = abs(adiff) st = g.anglefactor * aval if st > 100: st = 100 st = asgn * g.speedsign * st if False: st_d = st - g.steering if st_d > 10: st = g.steering + 10 elif st_d < -10: st = g.steering - 10 driving.steer(st) tolog("gotoa4 steer %f" % (st)) if not g.simulate: nav_tc.send_to_ground_control( "dpos %f %f %f %f 0 %f" % (g.ppx, g.ppy, g.ang, time.time() - g.t0, g.finspeed)) tt0 = time.time() d = nav_map.roaddist(g.ppx, g.ppy) if d > g.slightlyoffroad: # print("roaddist %f at %f, %f" % (d, g.ppx, g.ppy)) if d > g.maxoffroad: print("roaddist %f at %f, %f" % (d, g.ppx, g.ppy)) return 2 period = 0.1 * g.speedfactor tt1 = time.time() dtt = tt1 - tt0 dtt1 = period - dtt if dtt1 > 0: time.sleep(period)