def _speed_picar_to_world(self, v_picar, m=215, b=0):
'''
Map picar +/- 0-100 speed to real-world speed in meters-per-second based on calibration.
'''
sgn = sign(v_picar)
v_world = (abs(v_picar) + b) / m
return sgn * v_world
def I(self, error=0, dt=None):
'''
Integral term
'''
if dt is None:
dt = self.dt
# If we have defined an active region for the integral controller,
# only use integral control when error is within that region.
# Helps control integral windup.
if (self.integral_active_region is not None):
# Check if you are outside integral_active_region
if (abs(error) > abs(self.integral_active_region)):
# You're in the inactive region. Return no control signal.
return 0
# If you've made it here, you either have no defined active region,
# or you are within that active region. Proceed as normal.
# Update integral_error
self.integral_error += error
# If integral_max is defined, limit integral windup
if self.integral_max is not None:
# Don't let integral pass limit
if abs(self.integral_error) > self.integral_max:
self.integral_error = sign(
self.integral_error) * self.integral_max
return self.Ki * self.integral_error * self.dt
def _steering_world_to_picar(self,
gamma_world,
m_right=1,
b_right=0,
m_left=None,
b_left=None):
'''
Map real-world turn-gamma in radians to picar turn-gamma in degrees based on calibration.
'''
ang = -gamma * 180 / pi # picar deals in degrees, and treats left (CCW) as negative
if m_left is None:
m_left = m_right
if b_left is None:
b_left = b_right
# Turn right
if ang > 0:
ang = int(m_right * ang + b_right)
else:
ang = int(m_left * ang + b_left)
# bound gamma between -max and +max
if abs(ang) > self.MAX_PICAR_TURN:
ang = sign(ang) * self.MAX_PICAR_TURN
return ang
def set_speed(self, v, units="world"):
direction = sign(v)
picar_speed = self._v_to_picar_speed(abs(v))
if picar_speed > self.MAX_PICAR_SPEED:
picar_speed = self.MAX_PICAR_SPEED
v = self._picar_turn_to_gamma(picar_speed)
self._set_v(v*direction)
def turn(self, picar_angle, apply=True):
'''
Sets picar steering to PICAR ANGLE in PICAR UNITS
'''
if abs(picar_turn) > self.MAX_PICAR_TURN:
picar_turn = sign(picar_turn) * self.MAX_PICAR_TURN
self._steering = int(picar_angle)
if apply:
self._apply_current_controls()
def guess(self, inputs):
sum = 0
for i in range(0, self.weights.__len__()):
sum += inputs[i] * self.weights[i]
# Call to activation function
output = sign(sum)
return output
def _speed_world_to_picar(self, v_world, m=215, b=0):
'''
Map real-world speed in meters-per-second to 0-100 picar speed based on calibration.
'''
sgn = sign(v_world)
v_picar = int(m * abs(v_world) + b)
v_picar = min(
max(v_picar, 0),
self.MAX_PICAR_SPEED) # bound speed between 0 and MAX_PICAR_SPEED
return sgn * v_picar
def update_aim(self, dt, events):
speed = 7
da = self.aiming - self.aimingness
self.aimingness += h.sign(da) * dt * speed
if self.aimingness > 1:
self.aimingness = 1
elif self.aimingness < 0:
self.aimingness = 0
self.shade.set_alpha(128 * self.aimingness)
self.slowdown = 1 - 0.95 * self.aimingness
def _send_steer(self, steer):
'''
Input steer in HARDWARE UNITS.
Sends control signal to hardware.
'''
# Set picar steering angle and bound by max steer
if abs(steer) > self.max_steer:
steer = sign(steer) * self.max_steer
self.turn_wheels(steer)
return steer # Returns value of steer, which MAY HAVE BEEN CHANGED if over max steer
def GAMMA(self,alpha,beta,dt):
rho = norm(self.rho[:2])
a = self.Kpa*alpha # @TODO: Integrate PID class
b = self.Kpb*beta # @TODO: Integrate PID class
# gamma = rho*a + b+2*self.Kpb/rho -b*(self.last_rho-rho)
gamma = a + b
gamma = angle_a2b(a=0, b=gamma)
# Stop at max
if abs(gamma) > self.MAX_TURNING_ANGLE:
gamma = sign(gamma)*self.MAX_TURNING_ANGLE
return gamma
def turn(self, gamma, units="world"):
if units == "world":
picar_turn = self._gamma_to_picar_turn(gamma)
if abs(picar_turn) > self.MAX_PICAR_TURN:
picar_turn = sign(picar_turn)*self.MAX_PICAR_TURN
gamma = self._picar_turn_to_gamma(picar_turn)
elif units == "picar":
picar_turn = gamma
else:
raise InputError("Invalid units arg {}. units must be 'world' or 'picar'.".format(units))
self._turn_wheels(int(picar_turn))
def _set_v(self, speed):
self._direction = sign(speed)
if self._direction == 1:
self._bw.forward()
elif self._direction == -1:
self._bw.backward()
else:
self.halt()
return
self._v= abs(speed)
self._bw.speed = abs(speed)
def go_to_point(self, point, pos_x_goal, yaw_goal):
"""
Simple P controller with extra if statements
:param point: shapely Point
:param pos_x_goal: goal position x (forward)
:param yaw_goal: goal yaw angle
:return: True if goal is withing all tolerances
"""
pos_error = point.x - pos_x_goal
yaw_error = math.atan2(point.y, point.x) - yaw_goal
# print("yaw_err: {:.2f}, pos_err: {:.2f}".format(math.degrees(yaw_error), pos_error))
# P controller with clampping
vel = 0
rot = 0
is_in_yaw_tol = abs(yaw_error) < self.yaw_tolerance
is_in_pos_tol = abs(pos_error) < self.pos_tolerance
is_far = pos_error > self.pos_far
# Align angle if not in tolerance
if not is_in_yaw_tol:
rot = yaw_error * self.yaw_p
rot = sign(rot) * clamp(abs(rot), self.yaw_min, self.yaw_max)
# Align position (when yaw in tolerance or really far away)
if (not is_in_pos_tol and is_in_yaw_tol) or is_far:
vel = pos_error * self.pos_p
vel = sign(vel) * clamp(abs(vel), self.pos_min, self.pos_max)
if is_far: # Speed up
vel *= 1.5
rot *= 1.5
self.drive(vel, rot)
if is_in_yaw_tol is True and is_in_pos_tol:
return True
return False
def _send_speed(self, speed):
'''
Input speed in HARDWARE UNITS.
Sends control signal to hardware.
'''
# Set picar speed
if speed == 0:
self.stop_motors()
else:
# Bound by max speed
if abs(speed) > self.max_speed:
speed = sign(speed) * self.max_speed
self.bw.speed = speed
return speed # Returns value of speed, which MAY HAVE BEEN CHANGED if over max speed
def set_speed(self, picar_speed, direction=None, apply=True):
'''
Sets picar speed to PICAR SPEED in PICAR UNITS.
Positive speeds are forward and negative speeds are backward.
'''
if direction is not None:
self._direction = direction
else:
self._direction = sign(picar_speed)
picar_speed = abs(picar_speed)
if picar_speed > self.MAX_PICAR_SPEED:
picar_speed = self.MAX_PICAR_SPEED
self._speed = int(picar_speed)
if apply:
self._apply_current_controls()
def qr(self, B=None): a = self n = a.rows I = HashMatrix.ident(n) q = I for r in range(n - 1): s = sum(a[i, r] ** 2 for i in range(r, n)) if s <= epsilon: break k = -sign(a[r]) * s ** 0.5 b = s - k * a[r] u = [0] * r + [a[r] - k] + \ [a[i, r] for i in range(r + 1, n)] u = HashMatrix.outer(u, u) P = I - b ** -1 * u a = P * a q = P * q if B is None: return q.trans(), a else: return a, q * B return q.trans(), a if B is None \ else a, q * B
def _apply_current_controls(self):
'''
Send current v and gamma control signals to hardware.
'''
# Capture current control values
direction = sign(self._direction)
gamma = direction * int(
self._steering) # reverse turn direction if we are going backward
speed = int(abs(self._speed))
# Set back wheel direction
if direction == 1:
self._bw.forward()
elif direction == -1:
self._bw.backward()
elif direction == 0:
# If direction is 0, don't change _bw's settings.
pass
else:
# If any other edge case, halt picar
self.halt()
print(
'Picar object has invalid direction field {}. Picar halting.'.
format(self._direction))
# Set picar steering angle
if gamma == 0:
self._turn_wheels_straight()
else:
self._turn_wheels(gamma)
# Set picar speed
if speed == 0:
self.stop_motors()
else:
self._bw.speed = speed
def main():
"""
Part-1: Payment Request Generation
"""
print("-------------------------------------------------------")
print("---PAYMENT REQUEST GENERATION---")
cust_privkey, cust_pubkey = rsakeys()
bank_privkey, bank_pubkey = rsakeys()
print("Customer's private key-", cust_privkey)
print("Customer's public key-", cust_pubkey)
print("Bank's private key-", bank_privkey)
print("Bank's public key-", bank_pubkey)
payment_info = 'Some payment information'
order_info = 'Some order information'
PIMD = get_hash(payment_info)
OIMD = get_hash(order_info)
POMD = get_hash(PIMD + OIMD)
dual_sign = sign(cust_privkey, POMD)
key_s = aeskey()
encrypted_pi, iv_pi = aesencrypt(payment_info, key_s)
encrypted_oimd, iv_oimd = aesencrypt(OIMD, key_s)
encrypted_ds, iv_ds = aesencrypt(dual_sign, key_s)
digital_envelope = encrypt(bank_pubkey, key_s)
"""
Part 2: Purchase Request Validation from
Merchant side
"""
merchant_oimd = get_hash(order_info)
merchant_pomd = get_hash(PIMD + merchant_oimd)
check_sign_merchant = verify(cust_pubkey, merchant_pomd, dual_sign)
if check_sign_merchant:
print("-------------------------------------------------------")
print("[INFO] Merchant Signatures match")
print("\tPurchase request validated by merchant")
print("\t---PURCHASE REQUEST VALIDATED---")
print("-------------------------------------------------------")
else:
print("-------------------------------------------------------")
print("[INFO] Signatures do not match")
print("\tPurchace request rejected- Signatures do not match!!")
return
"""
Part 3: Payment authorization
"""
bank_key_s = decrypt(bank_privkey, digital_envelope)
bank_pi = aesdecrypt(encrypted_pi, bank_key_s, iv_pi).decode()
bank_oimd = aesdecrypt(encrypted_oimd, bank_key_s, iv_oimd).decode()
bank_ds = aesdecrypt(encrypted_ds, bank_key_s, iv_ds)
bank_pimd = get_hash(bank_pi)
bank_pomd = get_hash(bank_pimd + bank_oimd)
check_sign_bank = verify(cust_pubkey, bank_pomd, dual_sign)
if check_sign_bank:
print("[INFO] Bank Signatures match")
print("\tPayment authorized by the bank")
print("\t---PAYMENT AUTHORIZATION SUCCESSFUL---")
print("-------------------------------------------------------")
print("\t---PAYMENT CAPTURE SUCCESSFUL---")
print("-------------------------------------------------------")
else:
print("[INFO] Signatures do not match")
print("\tPayment authorization failed- Signatures do not match!!")
def dYdt(speed, heading, direction=1):
v = sign(direction) * abs(speed)
return v * sin(heading)
def dXdt(speed, heading, direction=1):
v = sign(direction) * abs(speed)
return v * cos(heading)
def dHdt(speed, steer, L, direction=1):
v = sign(direction) * abs(speed)
return within_pi(v * tan(steer) / L)
def dBETAdt(rho, alpha, speed, direction=1):
v = sign(direction) * abs(speed)
return within_pi(-v * cos(alpha) / rho)
def dALPHAdt(rho, alpha, speed, steer, direction=1):
v = sign(direction) * abs(speed)
return within_pi(v * sin(alpha) / rho - steer)
def dRHOdt(alpha, speed, direction=1):
v = sign(direction) * abs(speed)
return -v * cos(alpha)
# steer.set_angle(steer.right_border / 3)
# # print(steer.left_border / 3, sensor_front.distance())
# elif dist_right - dist_left > 5:
# if dist_left < 100:
# # engine.run(100)
# steer.set_angle(steer.left_border)
# # print(steer.left_border, sensor_front.distance())
# elif dist_left < 300:
# # engine.run(200)
# steer.set_angle(steer.left_border / 2)
# # print(steer.left_border / 2, sensor_front.distance())
# elif dist_left < 600:
# # engine.run(250)
# steer.set_angle(steer.left_border / 3)
# # print(steer.left_border / 3, sensor_front.distance())
# else:
# steer.set_angle(0)
# print(sensor_front.distance())
if (engine.stalled() or sensor_front.distance() < 100
or sensor_right.distance() < 10 or sensor_left.distance() < 10
or sensor_right.distance() == 2550
or sensor_left.distance() == 2550):
steer.set_angle(steer.left_border * sign(delta))
engine.run_time(-300, 700)
engine.run(200)
# engine.stop(Stop.BREAK)
