-
Notifications
You must be signed in to change notification settings - Fork 0
/
LineSegment.py
51 lines (39 loc) · 1.65 KB
/
LineSegment.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
import Robot
import numpy as np
from math import atan2, sqrt
import config
class LineSegment:
def __init__(self, start, stop, velocity):
self.start = start
self.stop = stop
self.setVelocityConstraints(velocity, velocity, 0)
def __init__(self, start, stop, vi, vf, acc):
self.start = start
self.stop = stop
self.setVelocityConstraints(vi, vf, acc)
def setVelocityConstraints(self, vi, vf, acc):
self.vel = vi
self.vf = vf
self.acc = acc
def vector(self):
return self.stop - self.start
def update(self, dt):
goalFromRobot = Robot.transformPointToRobot(self.stop)
if self.acc is not 0:
remainingLength = sqrt(goalFromRobot[0]**2 + goalFromRobot[1]**2)
deltaVel = self.acc * dt
# If we're within one timestep of the goal velocity, just jump to that velocity
# Prevents oscilations because we move above the goal within one timestep
if abs(self.vf-self.vel) <= deltaVel:
self.vel = self.vf
# If we're under the target velocity
elif self.vel < self.vf:
self.vel += deltaVel
if self.vf == 0:
# How fast can we go while respecting acceleration limits for slowing down in time
maxVelocity = sqrt(2*self.acc*remainingLength)
# Clamp current velocity to the max possible
if self.vel > maxVelocity:
self.vel = maxVelocity
Robot.sendDifferentialDriveCmd(self.vel, goalFromRobot[1]*config.KP_HEADING)
return abs(goalFromRobot[0]) > 0.01