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)