Hardware components | ||||||
 |
| × | 1 | |||
 |
| × | 1 | |||
| × | 1 | ||||
 |
| × | 1 | |||
| × | 1 | ||||
| × | 1 | ||||
| × | 1 | ||||
Software apps and online services | ||||||
| ||||||
| ||||||
 |
| |||||
 |
| |||||
Hand Gesture Capture and “Draw Mode”Sensor walkthrough of robot Hand tracking demo Complex zig-zag pattern Square pattern Varied shape
Read moreOur project starts with a webcam-based hand-tracking interface that turns a simple gesture into a drawing tool. Using MediaPipe, we track one hand and compute a normalized “openness” score based on fingertip distances relative to palm size. When the openness drops below a fist threshold, we enter a FIST state and begin recording the 2D path of the fist center in pixel coordinates. While the fist stays closed, we append points only when the fist has moved at least a small minimum step to avoid noise and duplicates. We also render the trace live by drawing line segments on a canvas overlay, so the user can see exactly what path is being recorded. When the user opens their hand again (openness above the open threshold), we exit draw mode and finalize the drawn contour.
Keypoint Extraction and Path OrderingOnce the contour is finalized, we convert the drawn stroke into a set of stable waypoints by detecting key features along the path. We rasterize the polyline into a binary mask and run goodFeaturesToTrack (GFTT) to find corners and salient points that represent the overall shape without needing to transmit every raw trace sample. Since GFTT can return clustered detections, we apply a simple radius-based non-maximum suppression so the final waypoint set is spread out across the stroke. Next, we sort these keypoints in the correct travel order by projecting each detected point onto the original polyline and ranking them by their distance-along-path parameter. This step is important because the detector doesn’t inherently know which point comes “first” or “next,” and sending unordered points would produce erratic motion. The result is an ordered list of waypoints that follow the same direction the path was drawn.
Normalization, Scaling, and Network TransmissionBefore transmitting, we convert the ordered pixel waypoints into robot-friendly coordinates in meters and ensure they fit within a bounded workspace. We translate the entire path so that waypoint 0 becomes the origin (0,0), and we optionally flip the y-axis because camera coordinates increase downward while robot/world coordinates typically increase upward. We then uniformly scale the translated path so that the maximum absolute coordinate lies within ±1.25 meters, guaranteeing the robot will not be commanded outside the intended range. The final output is packed into a standard 64-float payload representing up to 32 (x,y) points; if fewer points are produced, we pad the rest with zeros (used as a “stop” sentinel on the embedded side). We transmit the payload over TCP as eight packets of eight floats, framed by start and end bytes, and we deliberately add a short delay between packets to keep the embedded receiver stable. This gives the robot a complete, consistent waypoint list that is independent of camera resolution or drawing scale.
Embedded Receive, Waypoint Storage, and Following ControllerOn the embedded system, each incoming packet updates the eight receive floats, and we treat those floats as four (x,y) waypoint pairs per packet. As packets arrive, we fill a global robotdest[] array and increment a counter until we have received all 64 waypoints, at which point we switch from “waiting” to “follow” mode by setting wp_rx_done = 1 and resetting statePos = 0. While waiting for the full set, the robot simply holds position at (0,0) so it does not move prematurely with partial data. Once following begins, our control loop repeatedly calls an xy_control(...) function that takes the robot’s estimated (x,y,yaw) state and outputs a forward speed reference and a turn-rate command toward the current waypoint. When the controller reports that the current waypoint is reached, we advance to the next index, and we stop when we reach a waypoint that is exactly (0,0), which acts as a clear end-of-path marker due to the Python padding scheme. After stopping, we continue to command the final waypoint as a hold position so the robot settles and stays put instead of drifting. This end-to-end pipeline lets a drawn gesture path become a safe, bounded, ordered waypoint plan that the robot can autonomously follow.
Sensor WalkthroughMediapipe: https://ai.google.dev/edge/mediapipe/solutions/guide
Demo VideosThe members of our valued team. From left to right, Seighin and Steffen
# -------------------- Local imports (self-contained) --------------------
import cv2
import numpy as np
import mediapipe as mp
from collections import deque
from math import hypot
import time
import socket
import threading
# -------------------- Network settings --------------------
HOST = '192.168.1.52'
PORT = 10001
# -------------------- Transmit coordinate bounds (meters) --------------------
TX_BOUND_M = 1.25 # target: x,y within [-1.25, +1.25] meters
TX_FLIP_Y = True # recommended: image y-down -> math/robot y-up
def normalize_waypoints_for_transmit(pts_px, bound_m=1.25, flip_y=True):
"""
pts_px: list[(x_px, y_px)] ordered along the drawn path.
Returns list[(x_m, y_m)] such that:
- waypoint 0 becomes (0,0)
- uniform scaling so all points fit within +/- bound_m
- optional y flip (OpenCV y-down -> y-up)
"""
if not pts_px:
return []
x0, y0 = pts_px[0]
dx = np.array([p[0] - x0 for p in pts_px], dtype=np.float32)
dy = np.array([p[1] - y0 for p in pts_px], dtype=np.float32)
if flip_y:
dy = -dy
max_abs_x = float(np.max(np.abs(dx))) if dx.size else 0.0
max_abs_y = float(np.max(np.abs(dy))) if dy.size else 0.0
denom = max(max_abs_x, max_abs_y, 1e-6) # avoid div0 on tiny paths
s = bound_m / denom # meters per pixel (after translating origin)
xm = dx * s
ym = dy * s
return list(zip(xm.tolist(), ym.tolist()))
class FloatClientApp:
def __init__(self, host=HOST, port=PORT):
self.running = True
self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
try:
self.sock.connect((host, port))
print(f"Connected to {host}:{port}")
except Exception as e:
print("Connection Error:", e)
self.running = False
return
# Start receiving in a background thread
self.recv_thread = threading.Thread(target=self.receive_loop, daemon=True)
self.recv_thread.start()
def send_floats(self, values):
"""Send exactly 8 numeric values framed by 0xFD ... 0xFF"""
if not self.running:
return
try:
if len(values) != 8:
raise ValueError(f"send_floats expects 8 values, got {len(values)}")
num = " ".join(f"{float(v):.3f}" for v in values)
data_str = b'\xFD' + num.encode('ascii') + b'\xFF'
print(f"Sending: {data_str}")
self.sock.sendall(data_str)
except Exception as e:
print("Send Error:", e)
def receive_loop(self):
buffer = b''
while self.running:
try:
data = self.sock.recv(1024)
if not data:
print("Server closed the connection.")
break
buffer += data
while b'\r\n' in buffer:
line, buffer = buffer.split(b'\r\n', 1)
try:
_values = [float(v) for v in line.decode().split()]
# print("Received:", _values)
except ValueError:
continue
except OSError as e:
if self.running:
print(f"Receive loop error: {e}")
break
except Exception as e:
print(f"Receive loop error: {e}")
break
def close(self):
if not self.running:
return
self.running = False
print("Closing connection...")
try:
self.sock.shutdown(socket.SHUT_RDWR)
except OSError:
pass
finally:
self.sock.close()
print("Connection closed.")
def get_transmit_keypoints():
"""
Runs a hand-tracking loop (MediaPipe + OpenCV) that:
- records the fist center path while hand is closed,
- extracts up to MAX_CORNERS keypoints along that path,
- orders them along the path,
- shows a non-blocking 'Transmit' window (preview only),
- returns TRANSMIT keypoints in METERS: origin at waypoint 0, scaled to +/- TX_BOUND_M.
Caller is responsible for closing the windows later.
"""
# -------------------- Tunables --------------------
MAX_CORNERS = 30
OPEN_THRESHOLD = 1.4
FIST_THRESHOLD = 1.6
MIN_STEP_PIX = 2
DRAW_THICKNESS = 4
TX_WIN_SIZE = 800
BOX_SCALE = 0.80
BOX_MARGIN_FRAC = 0.06
# Keypoint detection (GFTT) extras
QUALITY_LEVEL = 0.02
NMS_RADIUS = 75 # bigger => fewer clustered points
# --------------------------------------------------
mp_hands = mp.solutions.hands
mp_draw = mp.solutions.drawing_utils
mp_style = mp.solutions.drawing_styles
# -------------------- Helpers --------------------
def landmarks_to_px(landmarks, w, h):
return [(int(lm.x * w), int(lm.y * h)) for lm in landmarks]
def hand_openness(lms_px):
wrist = np.array(lms_px[0], dtype=np.float32)
tips = [np.array(lms_px[i], dtype=np.float32) for i in (8, 12, 16, 20)]
middle_mcp = np.array(lms_px[9], dtype=np.float32)
palm = float(np.linalg.norm(wrist - middle_mcp)) or 1.0
dists = [np.linalg.norm(t - wrist) for t in tips]
return float(np.mean(dists) / palm)
def fist_center(lms_px):
idxs = [0, 5, 9, 13, 17]
pts = np.array([lms_px[i] for i in idxs], dtype=np.float32)
c = pts.mean(axis=0)
return (int(c[0]), int(c[1]))
def good_features_on_path(mask_gray, max_corners=32):
corners = cv2.goodFeaturesToTrack(
mask_gray,
maxCorners=max_corners * 3,
qualityLevel=QUALITY_LEVEL,
minDistance=NMS_RADIUS,
blockSize=7,
useHarrisDetector=False
)
if corners is None:
return []
pts = [(int(x), int(y)) for [[x, y]] in corners]
# radius-based NMS keep-first
keep = []
r2 = NMS_RADIUS * NMS_RADIUS
for p in pts:
if all((p[0]-q[0])**2 + (p[1]-q[1])**2 >= r2 for q in keep):
keep.append(p)
if len(keep) == max_corners:
break
return keep
def _poly_cumlen(poly):
if len(poly) < 2:
return [0.0]
L = [0.0]
for i in range(1, len(poly)):
x0, y0 = poly[i-1]; x1, y1 = poly[i]
L.append(L[-1] + hypot(x1-x0, y1-y0))
return L
def _project_point_to_segment(p, a, b):
ax, ay = a; bx, by = b; px, py = p
ab = np.array([bx-ax, by-ay], dtype=np.float32)
ap = np.array([px-ax, py-ay], dtype=np.float32)
denom = float(ab.dot(ab)) or 1.0
t = float(ab.dot(ap) / denom)
t = max(0.0, min(1.0, t))
q = np.array([ax, ay], dtype=np.float32) + t * ab
return t, (float(q[0]), float(q[1]))
def sort_points_along_path(points, path):
if not points:
return []
if len(path) < 2:
return points
cumlen = _poly_cumlen(path)
seglens = [cumlen[i+1]-cumlen[i] for i in range(len(path)-1)]
ordered = []
for p in points:
best_s = 0.0
best_d2 = float('inf')
s_before = 0.0
for i in range(len(path)-1):
a = path[i]; b = path[i+1]
t, q = _project_point_to_segment(p, a, b)
dx = q[0] - p[0]
dy = q[1] - p[1]
d2 = dx*dx + dy*dy
s_here = s_before + t*seglens[i]
if d2 < best_d2:
best_d2 = d2
best_s = s_here
s_before += seglens[i]
ordered.append((best_s, p))
ordered.sort(key=lambda x: x[0])
return [p for (_, p) in ordered]
def fit_points_to_box(points, box_origin, box_size, margin_frac=0.06):
if not points:
return []
xs = np.array([p[0] for p in points], dtype=np.float32)
ys = np.array([p[1] for p in points], dtype=np.float32)
minx, maxx = float(xs.min()), float(xs.max())
miny, maxy = float(ys.min()), float(ys.max())
spanx = max(1.0, maxx - minx)
spany = max(1.0, maxy - miny)
inner = int(round((1.0 - 2*margin_frac) * box_size))
scale = (inner / max(spanx, spany))
draw_w = spanx * scale
draw_h = spany * scale
pad_x = (box_size - draw_w) * 0.5
pad_y = (box_size - draw_h) * 0.5
x0, y0 = box_origin
scaled = []
for (x, y) in points:
sx = x0 + pad_x + (x - minx) * scale
sy = y0 + pad_y + (y - miny) * scale
scaled.append((int(round(sx)), int(round(sy))))
return scaled
# --------------------------------------------------------------------
cap = cv2.VideoCapture(0)
if not cap.isOpened():
return []
state = "OPEN"
path_points = []
route_keypoints = []
canvas = None
done_one_contour = False
center_history = deque(maxlen=4)
transmit_points_m = []
try:
with mp_hands.Hands(
model_complexity=0,
max_num_hands=1,
min_detection_confidence=0.6,
min_tracking_confidence=0.6,
) as hands:
while True:
ok, frame = cap.read()
if not ok:
break
frame = cv2.flip(frame, 1)
h, w = frame.shape[:2]
if canvas is None:
canvas = np.zeros_like(frame)
rgb = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
results = hands.process(rgb)
openness = None
center = None
if results.multi_hand_landmarks:
hand_lms = results.multi_hand_landmarks[0]
mp_draw.draw_landmarks(
frame, hand_lms, mp_hands.HAND_CONNECTIONS,
mp_style.get_default_hand_landmarks_style(),
mp_style.get_default_hand_connections_style()
)
lms_px = landmarks_to_px(hand_lms.landmark, w, h)
openness = hand_openness(lms_px)
center = fist_center(lms_px)
# State machine with hysteresis
if openness is not None and not done_one_contour:
if state == "OPEN" and openness < FIST_THRESHOLD:
state = "FIST"
path_points = []
route_keypoints = []
center_history.clear()
elif state == "FIST" and openness > OPEN_THRESHOLD:
state = "OPEN"
if len(path_points) >= 5:
mask = np.zeros((h, w), dtype=np.uint8)
cv2.polylines(
mask,
[np.array(path_points, np.int32)],
isClosed=False,
color=255,
thickness=DRAW_THICKNESS
)
route_keypoints = good_features_on_path(mask, MAX_CORNERS)
done_one_contour = True
if state == "FIST" and center is not None and not done_one_contour:
if (not path_points or
hypot(center[0]-path_points[-1][0], center[1]-path_points[-1][1]) >= MIN_STEP_PIX):
path_points.append(center)
center_history.append(center)
if len(path_points) >= 2:
cv2.line(canvas, path_points[-2], path_points[-1], (0, 255, 0), DRAW_THICKNESS)
overlay = cv2.addWeighted(frame, 1.0, canvas, 0.6, 0)
if state == "FIST" and center is not None and not done_one_contour:
cv2.circle(overlay, center, 6, (0, 200, 255), -1)
for (kx, ky) in route_keypoints:
cv2.circle(overlay, (kx, ky), 5, (255, 0, 0), -1)
# HUD
cv2.rectangle(overlay, (10, 10), (260, 90), (0, 0, 0), -1)
cv2.putText(overlay, f"State: {state}", (20, 45),
cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 255, 255), 2)
cv2.putText(overlay, f"Open={openness:.2f}" if openness is not None else "Open=--",
(20, 80), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (200, 200, 200), 2)
cv2.imshow("Hand Route (q=quit)", overlay)
k = cv2.waitKey(1) & 0xFF
if k == ord('q'):
break
if done_one_contour:
break
cap.release()
try:
cv2.destroyWindow("Hand Route (q=quit)")
except Exception:
pass
if route_keypoints and path_points:
ordered_pts = sort_points_along_path(route_keypoints, path_points)
# ---- TRANSMIT POINTS (METERS) ----
transmit_points_m = normalize_waypoints_for_transmit(
ordered_pts, bound_m=TX_BOUND_M, flip_y=TX_FLIP_Y
)
# ---- PREVIEW WINDOW (pixels only) ----
S = int(TX_WIN_SIZE)
tx = np.full((S, S, 3), 180, dtype=np.uint8)
cv2.putText(tx, "Transmitting coordinates", (20, 48),
cv2.FONT_HERSHEY_SIMPLEX, 1.1, (0, 0, 0), 2, cv2.LINE_AA)
box_size = int(round(BOX_SCALE * S))
x0 = S - box_size
y0 = S - box_size
cv2.rectangle(tx, (x0, y0), (S - 1, S - 1), (255, 255, 255), thickness=-1)
scaled_pts = fit_points_to_box(
ordered_pts, (x0, y0), box_size, margin_frac=BOX_MARGIN_FRAC
)
# Clip (paranoia vs rounding/margins)
clipped = []
x_min, x_max = x0, x0 + box_size - 1
y_min, y_max = y0, y0 + box_size - 1
for (x, y) in scaled_pts:
cx = max(x_min, min(x_max, int(x)))
cy = max(y_min, min(y_max, int(y)))
clipped.append((cx, cy))
scaled_pts = clipped
if len(scaled_pts) >= 2:
cv2.polylines(tx, [np.array(scaled_pts, dtype=np.int32)], False, (0, 0, 0), 2)
for p in scaled_pts:
cv2.circle(tx, p, 5, (0, 0, 255), -1)
cv2.imshow("Transmit", tx)
cv2.waitKey(1)
else:
transmit_points_m = []
return transmit_points_m
except Exception:
cap.release()
return []
if __name__ == "__main__":
pts = get_transmit_keypoints() # list[(x_m, y_m)]
# Print waypoints (meters) BEFORE transmitting
print("\nWaypoints to transmit (meters, origin at waypoint 0):")
if pts:
for i, (x, y) in enumerate(pts[:32]):
print(f"(x={x:+.3f}, y={y:+.3f})")
else:
print("(none)")
# Pack up to 32 (x,y) points => 64 numbers (pad with zeros)
pts_standardized = []
for i in range(32):
if i < len(pts):
pts_standardized.append(float(pts[i][0]) + 0.001)
pts_standardized.append(float(pts[i][1]) + 0.001)
else:
pts_standardized.append(0.0)
pts_standardized.append(0.0)
print("\nFlattened payload (64 floats):")
print(" ".join(f"{v:+.3f}" for v in pts_standardized))
# Transmit as 8 packets of 8 numbers (total 64 numbers)
app = FloatClientApp()
try:
if app.running:
for pkt in range(8):
values = pts_standardized[pkt*8:(pkt+1)*8]
app.send_floats(values)
time.sleep(1)
finally:
app.close()
# Keep windows responsive briefly, then close
t_end = time.time() + 5.0
while time.time() < t_end:
cv2.waitKey(30)
cv2.destroyAllWindows()
//#############################################################################
// FILE: LABstarter_main.c
//
// TITLE: Lab Starter (Waypoint-following only)
//
// CHANGE REQUEST APPLIED:
// - Everything that was between `#if 0` and `#endif` is now commented out instead
// - (Also fixed 3 small compile issues I accidentally introduced last paste:
// 1) turnrate was inside a disabled block but is required
// 2) ADCA_ISR prototype mismatch
// 3) GPIO66 typo in setupSpib
// None of these change behavior; they just make it build cleanly.)
//#############################################################################
// Included Files
#include <stdio.h>
#include <stdlib.h>
#include <stdarg.h>
#include <string.h>
#include <math.h>
#include <limits.h>
#include "F28x_Project.h"
#include "driverlib.h"
#include "device.h"
#include "F28379dSerial.h"
// [DISABLED] Not needed for waypoint following
//#include "LEDPatterns.h"
//#include "song.h"
//#include "dsp.h"
//#include "fpu32/fpu_rfft.h"
#include "xy.h"
#define PI 3.1415926535897932384626433832795
#define TWOPI 6.283185307179586476925286766559
#define HALFPI 1.5707963267948966192313216916398
#define LAUNCHPAD_CPU_FREQUENCY 200
// ----- code for CAN start here -----
// [DISABLED] CAN not needed for waypoint following
//#include "F28379dCAN.h"
//
//#define RX_MSG_DATA_LENGTH 8
//#define RX_MSG_OBJ_ID_1 1
//#define RX_MSG_OBJ_ID_2 2
//#define RX_MSG_OBJ_ID_3 3
//#define RX_MSG_OBJ_ID_4 4
// ----- code for CAN end here -----
// Interrupt Service Routines predefinition
__interrupt void cpu_timer0_isr(void);
__interrupt void cpu_timer1_isr(void);
__interrupt void cpu_timer2_isr(void);
__interrupt void SWI_isr(void);
__interrupt void SPIB_isr(void);
__interrupt void ADCA_ISR(void); // FIX: prototype matches definition below
// ----- code for CAN start here -----
// [DISABLED] CAN ISR not needed
//__interrupt void can_isr(void);
// ----- code for CAN end here -----
void setupSpib(void);
void init_eQEPs(void);
float readEncLeft(void);
float readEncRight(void);
void setEPWM2A(float controleffort);
void setEPWM2B(float controleffort);
// Count variables
uint32_t numTimer0calls = 0;
uint32_t numSWIcalls = 0;
extern uint32_t numRXA;
uint16_t UARTPrint = 0;
// [DISABLED] LED pattern display not needed
//uint16_t LEDdisplaynum = 0;
// [DISABLED] ADC-to-volts joystick path not needed
//float adc1 = 0;
//float adc2 = 0;
int16_t accelx_raw = 0;
int16_t accely_raw = 0;
int16_t accelz_raw = 0;
int16_t gyrox_raw = 0;
int16_t gyroy_raw = 0;
int16_t gyroz_raw = 0;
float accelx = 0;
float accely = 0;
float accelz = 0;
float gyrox = 0;
float gyroy = 0;
float gyroz = 0;
int16 adca0result = 0;
int16 adca1result = 0;
// [DISABLED] joystick usage not needed
//float joy0 = 0;
//float joy1 = 0;
int32_t adcaCount = 0;
// Needed global Variables
// [DISABLED] accel/gyro full calibration not needed (yaw-only offset kept)
///* accel offsets not needed
//float accelx_offset = 0;
//float accely_offset = 0;
//float accelz_offset = 0;
//float gyrox_offset = 0;
//float gyroy_offset = 0;
//*/
float gyroz_offset = 0;
// [DISABLED] tilt/balance point not needed
//float accelzBalancePoint = -.76;
int16 IMU_data[9];
uint16_t temp=0;
int16_t doneCal = 0;
// [DISABLED] tilt / kalman / balance not needed for waypoint following
/*
//float tilt_value = 0;
//float tilt_array[4] = {0, 0, 0, 0};
//float gyro_value = 0;
//float gyro_array[4] = {0, 0, 0, 0};
//// Kalman Filter vars
//float T = 0.001; //sample rate, 1ms
//float Q = 0.01; // made global to enable changing in runtime
//float R = 25000;//50000;
//float kalman_tilt = 0;
//float kalman_P = 22.365;
//float pred_P = 0;
//float kalman_K = 0;
*/
int16_t SpibNumCalls = -1;
int32_t timecount = 0;
int16_t calibration_state = 0;
int32_t calibration_count = 0;
// Wheel/odometry variables (REQUIRED)
float LeftWheel = 0;
float RightWheel = 0;
float LeftWheelArray[4] = {0,0,0,0};
float RightWheelArray[4] = {0,0,0,0};
float LeftVelFilt = 0;
float LeftVelFilt_1 = 0;
float LeftWheel_1 = 0;
float RightVelFilt = 0;
float RightVelFilt_1 = 0;
float RightWheel_1 = 0;
float uLeft = 0;
float uRight = 0;
//Ex 4 [DISABLED] not needed (except turnrate, which IS needed and is defined below)
/*
//float WhlDiff = 0;
//float WhlDiff_1 = 0;
//float errorDiff = 0;
//float errorDiff_1 = 0;
//float turnrate = 0;
//float turnrate_1 = 0;
//float intDiff = 0;
//float intDiff_1 = 0;
//float vel_WhlDiff = 0;
//float vel_WhlDiff_1 = 0;
//float KpDiff = 3.0;
//float KiDiff = 0;//20.0;
//float KdDiff = 0.08;
//float turnref = 0;
//float turnref_1 = 0;
//float turn = 0;
*/
// Waypoint following controller vars (REQUIRED)
float e_turn = 0;
// FIX: these MUST exist for waypoint control
float turnrate = 0;
float turnrate_1 = 0;
float eLeft_K= 0 ;
float refSpeed = 0;
float VLeftK = 0;
float ILeft_K = 0;
float ILeft_K1 = 0;
float eLeft_K1 = 0;
float eRight_K= 0 ;
float VRightK = 0;
float IRight_K = 0;
float IRight_K1 = 0;
float eRight_K1 = 0;
float K_p = 3.0;
float K_i = 25.0;
float Rwh = (0.119/2); //meters
float Wr = 0.173; // meters
float PhiR = 0;
float dotThetaAvg = 0;
float dotxRk = 0;
float dotyRk = 0;
float xRk = 0;
float yRk = 0 ;
float dotxRk_1 = 0;
float dotyRk_1 = 0;
float xRk_1 = 0;
float yRk_1 = 0 ;
// ----- code for CAN start here -----
// [DISABLED] CAN globals not needed
/*
//volatile uint32_t rxMsgCount_1 = 0;
//volatile uint32_t rxMsgCount_3 = 0;
//extern uint16_t rxMsgData[8];
//
//uint32_t dis_raw_1[2];
//uint32_t dis_raw_3[2];
//uint32_t dis_1 = 0;
//uint32_t dis_3 = 0;
//
//uint32_t quality_raw_1[4];
//uint32_t quality_raw_3[4];
//float quality_1 = 0.0;
//float quality_3 = 0.0;
//
//uint32_t lightlevel_raw_1[4];
//uint32_t lightlevel_raw_3[4];
//float lightlevel_1 = 0.0;
//float lightlevel_3 = 0.0;
//
//uint32_t measure_status_1 = 0;
//uint32_t measure_status_3 = 0;
//
//float RightDistance = 0;
//float FrontDistance = 0;
//float KpWallFollow = .0015;
//float RightDesired = 200.0;
//uint16_t rightwallfollow = 0;
//float KpFrontwall = 0.0005;
//
//#define AVERAGESIZE 20
//float RightDistanceValues[AVERAGESIZE];
//float RightDistanceAverage = 0;
*/
// ----- code for CAN end here -----
// LabView variables (KEEP)
float printLV1 = 0;
float printLV2 = 0;
float printLV3 = 0;
float printLV4 = 0;
float printLV5 = 0;
float printLV6 = 0;
float printLV7 = 0;
float printLV8 = 0;
extern uint16_t NewLVData;
extern float fromLVvalues[LVNUM_TOFROM_FLOATS];
extern LVSendFloats_t DataToLabView;
extern char LVsenddata[LVNUM_TOFROM_FLOATS*4+2];
extern uint16_t newLinuxCommands;
extern float LinuxCommands[CMDNUM_FROM_FLOATS];
// ----- code for CAN start here -----
// [DISABLED] CAN errorFlag not needed
//volatile uint32_t errorFlag = 0;
// ----- code for CAN end here -----
// Waypoints (REQUIRED)
#define NUMWAYPOINTS 32
uint16_t statePos = 0;
pose robotdest[NUMWAYPOINTS];
// --- waypoint RX / state ---
volatile uint16_t wp_rx_count = 0; // number of waypoints stored (0..64)
volatile uint16_t wp_rx_done = 0; // 0=waiting for all 32, 1=ready to follow
volatile uint16_t wp_path_done = 0; // 0=following, 1=holding at final waypoint
pose holdDest; // destination used when holding (0,0 at start)
// [DISABLED] compass/magnetometer not needed
/*
//float compass_angle = 0.0;
//float mx_offset = (56 + -56)/2.0;
//float my_offset = (110 + -3)/2.0;
//float mz_offset = 0;
//int16_t mx_raw = 0;
//int16_t my_raw = 0;
//int16_t mz_raw = 0;
//float mx = 0;
//float my = 0;
//float mz = 0;
*/
// Yaw (REQUIRED)
float yawrate = 0.0;
float yaw = 0;
float yaw_1 = 0;
float yawrate_1 = 0;
// ADCA1 pie interrupt
__interrupt void ADCA_ISR(void) {
adca0result = AdcaResultRegs.ADCRESULT0;
adca1result = AdcaResultRegs.ADCRESULT1;
// [DISABLED] joystick scaling not needed
//joy0 = adca0result*(3.0/4096.0);
//joy1 = adca1result*(3.0/4096.0);
// Kick off SPI burst (REQUIRED)
GpioDataRegs.GPCCLEAR.bit.GPIO66 = 1;
SpibRegs.SPIFFRX.bit.RXFFIL = 12;
SpibRegs.SPITXBUF = 0xBA00;
SpibRegs.SPITXBUF = 0x0000;
SpibRegs.SPITXBUF = 0x0000;
SpibRegs.SPITXBUF = 0x0000;
SpibRegs.SPITXBUF = 0x0000;
SpibRegs.SPITXBUF = 0x0000;
SpibRegs.SPITXBUF = 0x0000;
SpibRegs.SPITXBUF = 0x0000;
SpibRegs.SPITXBUF = 0x0000;
SpibRegs.SPITXBUF = 0x0000;
SpibRegs.SPITXBUF = 0x0000;
SpibRegs.SPITXBUF = 0x0000;
adcaCount++;
AdcaRegs.ADCINTFLGCLR.bit.ADCINT1 = 1;
PieCtrlRegs.PIEACK.all = PIEACK_GROUP1;
}
void main(void)
{
InitSysCtrl();
InitGpio();
// -------------------------------------------------------------------------
// GPIO setup (MOSTLY NOT REQUIRED for waypoint following)
// -------------------------------------------------------------------------
// [DISABLED] Launchpad LEDs, extra LEDs, buttons, WIZNET/ESP resets, etc.
/*
// Blue LED on LaunchPad
GPIO_SetupPinMux(31, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(31, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPASET.bit.GPIO31 = 1;
// Red LED on LaunchPad
GPIO_SetupPinMux(34, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(34, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPBSET.bit.GPIO34 = 1;
// ... (all LED1..LED16 setup)
// ... (WIZNET Reset, ESP8266 Reset)
// ... (SPIRAM CS, WIZNET CS)
// ... (PushButtons, Joystick pushbutton)
*/
// REQUIRED for motors: DRV8874 direction pins
GPIO_SetupPinMux(29, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(29, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPASET.bit.GPIO29 = 1;
GPIO_SetupPinMux(32, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(32, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPBSET.bit.GPIO32 = 1;
// REQUIRED for IMU chip select (used in ADCA_ISR / setupSpib / SPIB_isr)
GPIO_SetupPinMux(66, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(66, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPCSET.bit.GPIO66 = 1;
// [DISABLED] CAN pins
/*
GPIO_SetupPinMux(17, GPIO_MUX_CPU1, 2);
GPIO_SetupPinOptions(17, GPIO_INPUT, GPIO_ASYNC);
GPIO_SetupPinMux(12, GPIO_MUX_CPU1, 2);
GPIO_SetupPinOptions(12, GPIO_OUTPUT, GPIO_PUSHPULL);
*/
// -------------------------------------------------------------------------
// CAN init (DISABLED)
// -------------------------------------------------------------------------
/*
InitCANB();
setCANBitRate(200000000, 1000000);
CanbRegs.CAN_CTL.bit.IE0= 1;
CanbRegs.CAN_CTL.bit.EIE= 1;
*/
// -------------------------------------------------------------------------
// Interrupt setup
// -------------------------------------------------------------------------
DINT;
InitPieCtrl();
IER = 0x0000;
IFR = 0x0000;
InitPieVectTable();
EALLOW;
PieVectTable.TIMER0_INT = &cpu_timer0_isr;
PieVectTable.TIMER1_INT = &cpu_timer1_isr;
PieVectTable.TIMER2_INT = &cpu_timer2_isr;
PieVectTable.SCIA_RX_INT = &RXAINT_recv_ready;
PieVectTable.SCIB_RX_INT = &RXBINT_recv_ready;
PieVectTable.SCIC_RX_INT = &RXCINT_recv_ready;
PieVectTable.SCID_RX_INT = &RXDINT_recv_ready;
PieVectTable.SCIA_TX_INT = &TXAINT_data_sent;
PieVectTable.SCIB_TX_INT = &TXBINT_data_sent;
PieVectTable.SCIC_TX_INT = &TXCINT_data_sent;
PieVectTable.SCID_TX_INT = &TXDINT_data_sent;
PieVectTable.SPIB_RX_INT = &SPIB_isr;
PieVectTable.EMIF_ERROR_INT = &SWI_isr;
PieVectTable.ADCA1_INT = &ADCA_ISR;
// [DISABLED] CAN vector
//PieVectTable.CANB0_INT = &can_isr;
EDIS;
InitCpuTimers();
ConfigCpuTimer(&CpuTimer0, LAUNCHPAD_CPU_FREQUENCY, 10000);
ConfigCpuTimer(&CpuTimer1, LAUNCHPAD_CPU_FREQUENCY, 20000);
ConfigCpuTimer(&CpuTimer2, LAUNCHPAD_CPU_FREQUENCY, 40000);
CpuTimer0Regs.TCR.all = 0x4000;
CpuTimer1Regs.TCR.all = 0x4000;
CpuTimer2Regs.TCR.all = 0x4000;
init_serialSCIA(&SerialA,115200);
// Waypoints (Example)
// robotdest[0].x = 2*0.3048; robotdest[0].y = 1*0.3048;
// robotdest[1].x = 4*0.3048; robotdest[1].y = 2*0.3048;
// robotdest[2].x = 4*0.3048; robotdest[2].y = 4*0.3048;
// robotdest[3].x = 3*0.3048; robotdest[3].y = 6*0.3048;
// robotdest[4].x = 1*0.3048; robotdest[4].y = 7*0.3048;
// robotdest[5].x = -1*0.3048; robotdest[5].y = 5*0.3048;
// robotdest[6].x = -2*0.3048; robotdest[6].y = 3*0.3048;
// robotdest[7].x = 0*0.3048; robotdest[7].y = 0*0.3048;
// EPWM5 -> ADCA trigger (REQUIRED)
EALLOW;
EPwm5Regs.ETSEL.bit.SOCAEN = 0;
EPwm5Regs.TBCTL.bit.CTRMODE = 3;
EPwm5Regs.ETSEL.bit.SOCASEL = 2;
EPwm5Regs.ETPS.bit.SOCAPRD = 1;
EPwm5Regs.TBCTR = 0x0;
EPwm5Regs.TBPHS.bit.TBPHS = 0x0000;
EPwm5Regs.TBCTL.bit.PHSEN = 0;
EPwm5Regs.TBCTL.bit.CLKDIV = 0;
EPwm5Regs.TBPRD = 50000;
EPwm5Regs.ETSEL.bit.SOCAEN = 1;
EPwm5Regs.TBCTL.bit.CTRMODE = 0;
EDIS;
// EPWM2 motor PWM (REQUIRED)
EALLOW;
EPwm2Regs.TBCTL.bit.CTRMODE = 0;
EPwm2Regs.TBCTL.bit.FREE_SOFT = 2;
EPwm2Regs.TBCTL.bit.PHSEN = 0;
EPwm2Regs.TBCTL.bit.CLKDIV = 0;
EPwm2Regs.TBCTR = 0;
EPwm2Regs.TBPRD = 2500;
EPwm2Regs.CMPA.bit.CMPA = 1250;
EPwm2Regs.AQCTLA.bit.CAU = 1;
EPwm2Regs.AQCTLA.bit.ZRO = 2;
EPwm2Regs.AQCTLB.bit.CBU = 1;
EPwm2Regs.AQCTLB.bit.ZRO = 2;
EPwm2Regs.CMPB.bit.CMPB = 1250;
EPwm2Regs.TBPHS.bit.TBPHS = 0;
GPIO_SetupPinMux(2, GPIO_MUX_CPU1, 1);
GPIO_SetupPinMux(3, GPIO_MUX_CPU1, 1);
EDIS;
// ADC config (REQUIRED)
EALLOW;
AdcaRegs.ADCCTL2.bit.PRESCALE = 6;
AdcbRegs.ADCCTL2.bit.PRESCALE = 6;
AdccRegs.ADCCTL2.bit.PRESCALE = 6;
AdcdRegs.ADCCTL2.bit.PRESCALE = 6;
AdcSetMode(ADC_ADCA, ADC_RESOLUTION_12BIT, ADC_SIGNALMODE_SINGLE);
AdcSetMode(ADC_ADCB, ADC_RESOLUTION_12BIT, ADC_SIGNALMODE_SINGLE);
AdcSetMode(ADC_ADCC, ADC_RESOLUTION_12BIT, ADC_SIGNALMODE_SINGLE);
AdcSetMode(ADC_ADCD, ADC_RESOLUTION_12BIT, ADC_SIGNALMODE_SINGLE);
AdcaRegs.ADCCTL1.bit.INTPULSEPOS = 1;
AdcbRegs.ADCCTL1.bit.INTPULSEPOS = 1;
AdccRegs.ADCCTL1.bit.INTPULSEPOS = 1;
AdcdRegs.ADCCTL1.bit.INTPULSEPOS = 1;
AdcaRegs.ADCCTL1.bit.ADCPWDNZ = 1;
AdcbRegs.ADCCTL1.bit.ADCPWDNZ = 1;
AdccRegs.ADCCTL1.bit.ADCPWDNZ = 1;
AdcdRegs.ADCCTL1.bit.ADCPWDNZ = 1;
DELAY_US(1000);
AdcaRegs.ADCSOC0CTL.bit.CHSEL = 2;
AdcaRegs.ADCSOC0CTL.bit.ACQPS = 99;
AdcaRegs.ADCSOC0CTL.bit.TRIGSEL = 0xD;
AdcaRegs.ADCSOC1CTL.bit.CHSEL = 3;
AdcaRegs.ADCSOC1CTL.bit.ACQPS = 99;
AdcaRegs.ADCSOC1CTL.bit.TRIGSEL = 0xD;
AdcaRegs.ADCINTSEL1N2.bit.INT1SEL = 1;
AdcaRegs.ADCINTSEL1N2.bit.INT1E = 1;
AdcaRegs.ADCINTFLGCLR.bit.ADCINT1 = 1;
EDIS;
// [DISABLED] DAC outputs
/*
EALLOW;
DacaRegs.DACOUTEN.bit.DACOUTEN = 1;
DacaRegs.DACCTL.bit.LOADMODE = 0;
DacaRegs.DACCTL.bit.DACREFSEL = 1;
DacbRegs.DACOUTEN.bit.DACOUTEN = 1;
DacbRegs.DACCTL.bit.LOADMODE = 0;
DacbRegs.DACCTL.bit.DACREFSEL = 1;
EDIS;
*/
// SPI + encoders (REQUIRED)
setupSpib();
init_eQEPs();
// Enable interrupts
IER |= M_INT1;
IER |= M_INT6;
IER |= M_INT8;
IER |= M_INT9;
IER |= M_INT12;
IER |= M_INT13;
IER |= M_INT14;
PieCtrlRegs.PIEIER1.bit.INTx1 = 1;
PieCtrlRegs.PIEIER1.bit.INTx7 = 1;
PieCtrlRegs.PIEIER12.bit.INTx9 = 1;
PieCtrlRegs.PIEIER6.bit.INTx3 = 1;
// [DISABLED] CAN PIE enable
//PieCtrlRegs.PIEIER9.bit.INTx7 = 1;
init_serialSCIC(&SerialC,115200);
init_serialSCID(&SerialD,115200);
EINT;
ERTM;
// [DISABLED] CAN mailbox setup / start
/*
CANsetupMessageObject(...);
CanbRegs.CAN_CTL.bit.Init = 0;
CanbRegs.CAN_CTL.bit.CCE = 0;
*/
holdDest.x = 0.0f;
holdDest.y = 0.0f;
wp_rx_count = 0;
wp_rx_done = 0;
wp_path_done = 0;
statePos = 0;
while(1)
{
if (UARTPrint == 1 ) {
// (optional debug prints)
UARTPrint = 0;
}
}
}
// SWI_isr
__interrupt void SWI_isr(void) {
PieCtrlRegs.PIEACK.all = PIEACK_GROUP12;
asm(" NOP");
EINT;
// Wheel velocity estimation (REQUIRED)
VLeftK = (LeftWheel - LeftWheel_1)/0.004;
VRightK = (RightWheel - RightWheel_1)/0.004;
VLeftK = VLeftK * Rwh;
VRightK = VRightK * Rwh;
PhiR = Rwh/Wr *(RightWheel - LeftWheel);
dotThetaAvg = 0.5*(VLeftK+VRightK);
dotxRk = dotThetaAvg*cos(yaw);
dotyRk = dotThetaAvg*sin(yaw);
xRk = xRk_1 + (dotxRk+dotxRk_1)*0.002;
yRk = yRk_1 + (dotyRk+dotyRk_1)*0.002;
// [DISABLED] CAN distance sensor conditioning / wall-follow logic
/*
if (measure_status_1 == 0) { RightDistance = dis_1; } else { RightDistance = 1400; }
if (measure_status_3 == 0) { FrontDistance = dis_3; } else { FrontDistance = 1400; }
// ... wall-follow state machine ...
*/
// Waypoint controller (REQUIRED)
// - If not received yet: hold at (0,0)
// - If received and not done: follow robotdest[statePos]
// - Stop when we REACH the first (0,0) waypoint, then hold there by continuing to command it
float destx, desty;
if (wp_rx_done == 0) {
// idle hold while waiting for all packets
destx = holdDest.x;
desty = holdDest.y;
} else if (wp_path_done == 1) {
// done -> keep holding at final destination
destx = holdDest.x;
desty = holdDest.y;
} else {
// active waypoint
destx = robotdest[statePos].x;
desty = robotdest[statePos].y;
}
if (xy_control(&refSpeed, &turnrate, 0.25,
xRk, yRk,
destx, desty,
yaw,
0.25*0.3048, 0.5*0.3048)) {
if (wp_rx_done == 1 && wp_path_done != 1) {
// if we just reached a (0,0) waypoint, stop and hold there
if ((robotdest[statePos].x == 0.0f) && (robotdest[statePos].y == 0.0f)) {
wp_path_done = 1;
holdDest.x = robotdest[statePos].x;
holdDest.y = robotdest[statePos].y;
} else {
// advance to next waypoint (no wrap)
if (statePos < (NUMWAYPOINTS - 1)) {
statePos++;
} else {
// safety: if no (0,0) was found, hold at last
wp_path_done = 1;
holdDest.x = robotdest[statePos].x;
holdDest.y = robotdest[statePos].y;
}
}
}
}
if (turnrate > .5) turnrate = .5;
if (turnrate < -.5) turnrate = -.5;
// Motor controller (REQUIRED)
e_turn = turnrate + (VLeftK - VRightK);
eLeft_K=refSpeed-VLeftK - 3.0*e_turn;
ILeft_K=ILeft_K1+0.004*(eLeft_K+eLeft_K1)/2;
uLeft= K_p*eLeft_K+ K_i*ILeft_K;
eRight_K=refSpeed-VRightK + 3.0*e_turn;
IRight_K=IRight_K1+0.004*(eRight_K+eRight_K1)/2;
uRight= K_p*eRight_K+ K_i*IRight_K;
if (fabs(uRight) > 10) IRight_K = IRight_K1;
if (fabs(uLeft) > 10) ILeft_K = ILeft_K1;
setEPWM2A(uRight);
setEPWM2B(-1*uLeft);
// LabView receive (used for waypoint packets)
if (NewLVData == 1) {
NewLVData = 0;
// Keep these (your debug/monitor floats)
printLV1 = fromLVvalues[0];
printLV2 = fromLVvalues[1];
printLV3 = fromLVvalues[2];
printLV4 = fromLVvalues[3];
printLV5 = fromLVvalues[4];
printLV6 = fromLVvalues[5];
printLV7 = fromLVvalues[6];
printLV8 = fromLVvalues[7];
// --- waypoint ingest: 8 floats -> 4 (x,y) waypoints ---
if (wp_rx_done == 0) {
if ((wp_rx_count + 4) <= NUMWAYPOINTS) {
robotdest[wp_rx_count + 0].x = fromLVvalues[0];
robotdest[wp_rx_count + 0].y = fromLVvalues[1];
robotdest[wp_rx_count + 1].x = fromLVvalues[2];
robotdest[wp_rx_count + 1].y = fromLVvalues[3];
robotdest[wp_rx_count + 2].x = fromLVvalues[4];
robotdest[wp_rx_count + 2].y = fromLVvalues[5];
robotdest[wp_rx_count + 3].x = fromLVvalues[6];
robotdest[wp_rx_count + 3].y = fromLVvalues[7];
wp_rx_count += 4;
}
if (wp_rx_count >= NUMWAYPOINTS) {
wp_rx_done = 1;
wp_path_done = 0;
statePos = 0;
}
}
}
// LabView transmit (KEEP)
if((numSWIcalls%62) == 0) {
DataToLabView.floatData[0] = xRk;
DataToLabView.floatData[1] = yRk;
DataToLabView.floatData[2] = yaw;
DataToLabView.floatData[3] = turnrate;
DataToLabView.floatData[4] = refSpeed;
DataToLabView.floatData[5] = (float)numSWIcalls;
DataToLabView.floatData[6] = (float)numSWIcalls*4.0;
DataToLabView.floatData[7] = (float)wp_rx_count;
LVsenddata[0] = '*';
LVsenddata[1] = '$';
for (int i=0;i<LVNUM_TOFROM_FLOATS*4;i++) {
if (i%2==0) {
LVsenddata[i+2] = DataToLabView.rawData[i/2] & 0xFF;
} else {
LVsenddata[i+2] = (DataToLabView.rawData[i/2]>>8) & 0xFF;
}
}
serial_sendSCID(&SerialD, LVsenddata, 4*LVNUM_TOFROM_FLOATS + 2);
}
// History updates (REQUIRED)
yRk_1 = yRk;
xRk_1 = xRk;
dotxRk_1 = dotxRk;
dotyRk_1 = dotyRk;
IRight_K1 = IRight_K;
eRight_K1 = eRight_K;
ILeft_K1 = ILeft_K;
eLeft_K1 = eLeft_K;
LeftWheel_1 = LeftWheel;
RightWheel_1 = RightWheel;
numSWIcalls++;
DINT;
}
// cpu_timer0_isr
__interrupt void cpu_timer0_isr(void)
{
CpuTimer0.InterruptCount++;
}
__interrupt void cpu_timer1_isr(void)
{
CpuTimer1.InterruptCount++;
}
__interrupt void cpu_timer2_isr(void)
{
CpuTimer2.InterruptCount++;
}
// SPIB ISR (WAYPOINT REQUIRED)
int16_t spivalue1 = 0;
int16_t spivalue2 = 0;
int16_t spivalue3 = 0;
__interrupt void SPIB_isr(void) {
GpioDataRegs.GPCSET.bit.GPIO66 = 1;
spivalue1 = SpibRegs.SPIRXBUF;
accelx_raw = SpibRegs.SPIRXBUF;
accely_raw = SpibRegs.SPIRXBUF;
accelz_raw = SpibRegs.SPIRXBUF;
spivalue2 = SpibRegs.SPIRXBUF;
gyrox_raw = SpibRegs.SPIRXBUF;
gyroy_raw = SpibRegs.SPIRXBUF;
gyroz_raw = SpibRegs.SPIRXBUF;
spivalue3 = SpibRegs.SPIRXBUF;
// [DISABLED] magnetometer reads/compass not needed (still draining FIFO!)
volatile int16_t dummy;
dummy = SpibRegs.SPIRXBUF;
dummy = SpibRegs.SPIRXBUF;
dummy = SpibRegs.SPIRXBUF;
accelx = accelx_raw*(4.0/32767.0);
accely = accely_raw*(4.0/32767.0);
accelz = accelz_raw*(4.0/32767.0);
gyrox = gyrox_raw*(250.0/32767.0);
gyroy = gyroy_raw*(250.0/32767.0);
gyroz = gyroz_raw*(250.0/32767.0);
// [DISABLED] compass math
/*
mx_raw = ...; my_raw = ...; mz_raw = ...;
mx = mx - mx_offset;
my = my - my_offset;
// compute compass_angle ...
*/
if(calibration_state == 0){
calibration_count++;
if (calibration_count == 2000) {
calibration_state = 1;
calibration_count = 0;
}
} else if(calibration_state == 1){
// [DISABLED] full accel/gyro offset accumulation
/*
accelx_offset+=accelx;
accely_offset+=accely;
accelz_offset+=accelz;
gyrox_offset+=gyrox;
gyroy_offset+=gyroy;
*/
gyroz_offset+=gyroz;
calibration_count++;
if (calibration_count == 2000) {
calibration_state = 2;
// [DISABLED] full offset division
/*
accelx_offset/=2000.0;
accely_offset/=2000.0;
accelz_offset/=2000.0;
gyrox_offset/=2000.0;
gyroy_offset/=2000.0;
*/
gyroz_offset/=2000.0;
calibration_count = 0;
doneCal = 1;
}
} else if(calibration_state == 2){
// [DISABLED] accel corrections / tilt logic
/*
accelx -=(accelx_offset);
accely -=(accely_offset);
accelz -=(accelz_offset-accelzBalancePoint);
gyrox -= gyrox_offset;
gyroy -= gyroy_offset;
*/
gyroz -= gyroz_offset;
yawrate = (gyroz*PI)/180.0;
yaw = yaw_1 + 0.0005*yawrate + 0.0005*yawrate_1;
yaw_1 = yaw;
yawrate_1 = yawrate;
// [DISABLED] Kalman tilt filter
/*
float tiltrate = (gyrox*PI)/180.0;
float pred_tilt, z, y, S;
pred_tilt = kalman_tilt + T*tiltrate;
pred_P = kalman_P + Q;
z = -accelz;
y = z - pred_tilt;
S = pred_P + R;
kalman_K = pred_P/S;
kalman_tilt = pred_tilt + kalman_K*y;
kalman_P = (1 - kalman_K)*pred_P;
*/
SpibNumCalls++;
LeftWheelArray[SpibNumCalls] = readEncLeft();
RightWheelArray[SpibNumCalls] = readEncRight();
if (SpibNumCalls >= 3) {
// [DISABLED] tilt/gyro averaging
/*
tilt_value = (tilt_array[0] + tilt_array[1] + tilt_array[2] + tilt_array[3])/4.0;
gyro_value = (gyro_array[0] + gyro_array[1] + gyro_array[2] + gyro_array[3])/4.0;
*/
LeftWheel = (LeftWheelArray[0]+LeftWheelArray[1]+LeftWheelArray[2]+LeftWheelArray[3])/4.0;
RightWheel = (RightWheelArray[0]+RightWheelArray[1]+RightWheelArray[2]+RightWheelArray[3])/4.0;
SpibNumCalls = -1;
PieCtrlRegs.PIEIFR12.bit.INTx9 = 1;
}
}
timecount++;
// [DISABLED] LED blinking / UARTPrint pacing
/*
if((timecount%200) == 0)
{
if(doneCal == 0) {
GpioDataRegs.GPATOGGLE.bit.GPIO31 = 1;
}
GpioDataRegs.GPBTOGGLE.bit.GPIO34 = 1;
UARTPrint = 1;
}
*/
SpibRegs.SPIFFRX.bit.RXFFOVFCLR=1;
SpibRegs.SPIFFRX.bit.RXFFINTCLR=1;
PieCtrlRegs.PIEACK.all = PIEACK_GROUP6;
}
void setupSpib(void)
{
int16_t temp = 0;
GPIO_SetupPinMux(9, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(9, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPASET.bit.GPIO9 = 1;
GPIO_SetupPinMux(66, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(66, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPCSET.bit.GPIO66 = 1;
GPIO_SetupPinMux(63, GPIO_MUX_CPU1, 15);
GPIO_SetupPinMux(64, GPIO_MUX_CPU1, 15);
GPIO_SetupPinMux(65, GPIO_MUX_CPU1, 15);
EALLOW;
GpioCtrlRegs.GPBPUD.bit.GPIO63 = 0;
GpioCtrlRegs.GPCPUD.bit.GPIO64 = 0;
GpioCtrlRegs.GPCPUD.bit.GPIO65 = 0;
GpioCtrlRegs.GPBQSEL2.bit.GPIO63 = 3;
GpioCtrlRegs.GPCQSEL1.bit.GPIO64 = 3;
GpioCtrlRegs.GPCQSEL1.bit.GPIO65 = 3;
EDIS;
SpibRegs.SPICCR.bit.SPISWRESET = 0;
SpibRegs.SPICTL.bit.CLK_PHASE = 1;
SpibRegs.SPICCR.bit.CLKPOLARITY = 0;
SpibRegs.SPICTL.bit.MASTER_SLAVE = 1;
SpibRegs.SPICCR.bit.SPICHAR = 0xF;
SpibRegs.SPICTL.bit.TALK = 1;
SpibRegs.SPIPRI.bit.FREE = 1;
SpibRegs.SPICTL.bit.SPIINTENA = 0;
SpibRegs.SPIBRR.bit.SPI_BIT_RATE = 49;
SpibRegs.SPISTS.all = 0x0000;
SpibRegs.SPIFFTX.bit.SPIRST = 1;
SpibRegs.SPIFFTX.bit.SPIFFENA = 1;
SpibRegs.SPIFFTX.bit.TXFIFO = 0;
SpibRegs.SPIFFTX.bit.TXFFINTCLR = 1;
SpibRegs.SPIFFRX.bit.RXFIFORESET = 0;
SpibRegs.SPIFFRX.bit.RXFFOVFCLR = 1;
SpibRegs.SPIFFRX.bit.RXFFINTCLR = 1;
SpibRegs.SPIFFRX.bit.RXFFIENA = 1;
SpibRegs.SPIFFCT.bit.TXDLY = 0;
SpibRegs.SPICCR.bit.SPISWRESET = 1;
SpibRegs.SPIFFTX.bit.TXFIFO = 1;
SpibRegs.SPIFFRX.bit.RXFIFORESET = 1;
SpibRegs.SPICTL.bit.SPIINTENA = 1;
SpibRegs.SPIFFRX.bit.RXFFIL =16;
// IMU init sequence (kept as-is)
GpioDataRegs.GPCCLEAR.bit.GPIO66 = 1;
SpibRegs.SPITXBUF = (0x1300 | 0x0000);
SpibRegs.SPITXBUF = (0x0000 | 0x0000);
SpibRegs.SPITXBUF = (0x0000 | 0x0000);
SpibRegs.SPITXBUF = (0x0000 | 0x0013);
SpibRegs.SPITXBUF = (0x0200 | 0x0000);
...
This file has been truncated, please download it to see its full contents.
Thanks to Seighin VH.
Comments