Throughout the project, we used AI tools as a support resource for analysis, debugging, and documentation rather than as a replacement for our own design work. AI assistance was frequently used to help reason through bugs in our code, interpret compiler and runtime errors, and understand unexpected behavior in complex sections involving interrupts, networking, and real-time control, which significantly sped up the debugging process. We also relied on AI when working with the Pico W’s Wi Fi subsystem, using it to better understand how access point configuration choices such as channel selection, IP addressing, and network setup affected connection stability and reliability. This helped us experiment with and refine our network configuration to achieve more consistent connectivity between devices. In addition, AI was used to help format and organize the project website, including adjusting image placement, improving layout consistency, and refining captions and text for readability. All core system design, control logic, hardware integration, and final implementation decisions were ultimately developed, tested, and validated by our team.
pid_controller.c
/**
* V. Hunter Adams (vha3@cornell.edu)
*
* This demonstration utilizes the MPU6050.
* It gathers raw accelerometer/gyro measurements, scales
* them, and plots them to the VGA display. The top plot
* shows gyro measurements, bottom plot shows accelerometer
* measurements.
*
* HARDWARE CONNECTIONS
* - GPIO 16 ---> VGA Hsync
* - GPIO 17 ---> VGA Vsync
* - GPIO 18 ---> 470 ohm resistor ---> VGA Green
* - GPIO 19 ---> 330 ohm resistor ---> VGA Green
* - GPIO 20 ---> 330 ohm resistor ---> VGA Blue
* - GPIO 21 ---> 330 ohm resistor ---> VGA Red
* - RP2040 GND ---> VGA GND
* - GPIO 8 ---> MPU6050 SDA
* - GPIO 9 ---> MPU6050 SCL
* - 3.3v ---> MPU6050 VCC
* - RP2040 GND ---> MPU6050 GND
* - GPIO 14 ---> H-Bridge Forward
* - GPIO 15 ---> H-Bridge Backward
*
*
*
*/
// Include standard libraries
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
// Include PICO libraries
#include "pico/stdlib.h"
#include "pico/multicore.h"
// Include hardware libraries
#include "hardware/pwm.h"
#include "hardware/dma.h"
#include "hardware/irq.h"
#include "hardware/adc.h"
#include "hardware/pio.h"
#include "hardware/i2c.h"
#include "hardware/clocks.h"
// Include custom libraries
#include "vga16_graphics_v2.h"
#include "mpu6050.h"
#include "pt_cornell_rp2040_v1_4.h"
#include "dhcpserver/dhcpserver.h"
#include "dnsserver/dnsserver.h"
#include "hardware/sync.h"
#include "hardware/uart.h"
//#include "hardware/rtc.h"
#include "hardware/adc.h"
#include "pico/util/datetime.h"
#include "pico/cyw43_arch.h"
#include "pico/stdlib.h"
//#include "lwip/dns.h"
#include "lwip/pbuf.h"
#include "lwip/tcp.h"
#include <time.h>
// SERVER CODE
int access_led_state ;
//int temp_sensor ; // turning on the temp_sensor kills the network!
// user state variables
int user_number ;
int test_field ;
// gpio2 % intensity
int test2_field = 25 ;
int color, new_color = true ;
float temp ;
// =======================================
// access point setup
// copied from pico_examples git
// html was modified and expanded
// =======================================
// add a 'printf' to the line below to dump
// internal state info e.g.:
// #define DEBUG_printf printf
#define DEBUG_printf
#define ANGLE_printf printf
//
#include "dhcpserver/dhcpserver.h"
#include "dnsserver/dnsserver.h"
//
#define TCP_PORT 80
#define POLL_TIME_S 5
#define HTTP_GET "GET"
#define HTTP_RESPONSE_HEADERS "HTTP/1.1 %d OK\nContent-Length: %d\nContent-Type: text/html;\
charset=utf-8\nConnection: close\n\n"
// the html. format specs represent server state to be resolved at send time
// this is one part I changed and expanded from the demo code
// the one-line javascript is hacked slightly from
// https://stackoverflow.com/questions/74320571/inserting-oninput-to-an-input-field
// and allows the numerical value of the slider to change as yuo move the slider
#define CONTROL_TEST1_BODY \
"<html><body><b>PID Tuning - Onboard Pico-W</b>\
<p>Onboard Led is %s\
\
<form>\
\
<label for=\"motoren\">Enable Motors:</label>\
<select name=\"led\" id=\"motoren\" size=\"2\" onchange=\"submit();\">\
<option value=\"0\" %s>Motor Off</option>\
<option value=\"1\" %s>Motor On </option>\
</select>\
\
<svg width=\"20\" height=\"20\">\
<circle cx=\"10\" cy=\"10\" r=\"10\" stroke=\"green\" stroke-width=\"1\" fill=%s />\
</svg><p>\
<label for=\"kp\"> K_P :</label>\
<input type=\"number\" step=\"0.01\" id=\"kp\" name=\"kp\" value=%.2f min=\"0\" max=\"999\">\
<input type=\"submit\"> <br>\
<label for=\"ki\"> K_I :</label>\
<input type=\"number\" step=\"0.01\" id=\"ki\" name=\"ki\" value=%.2f min=\"0\" max=\"999\">\
<input type=\"submit\"> <br>\
<label for=\"kd\"> K_D :</label>\
<input type=\"number\" step=\"0.01\" id=\"kd\" name=\"kd\" value=%.2f min=\"0\" max=\"999\">\
<input type=\"submit\"> <br>\
<label for=\"spoint\"> Absolute SP: :</label>\
<input type=\"number\" step=\"0.01\" id=\"spoint\" name=\"spoint\" value=%.2f min=\"-20\" max=\"20\">\
<input type=\"submit\"> <br>\
Proportional:<b><font color=\"RED\"> %f </font></b> <br>\
Integral:<b><font color=\"RED\"> %f </font></b> <br> \
Derivative:<b><font color=\"RED\"> %f </font></b> <br>\
Absolute SP:<b><font color=\"RED\"> %f </font></b> <br>\
<p>\
</form>\
<p><a href=http://192.168.4.1/control_test2> Control Panel </a>\
</body></html>"
// this page uses javascript submit() method so a button is not necessary
// it also tests <svg> graphics to draw a dynamic LED icon
// <label for=\"led_control1\">led on/off:</label><br>\
// <input type=\"text\" id=\"led_control1\" name=\"led\" value=%d><br>\
#define CONTROL_TEST2_BODY \
"<html><body><b>Control Panel -- Onboard Pico-W</b>\
<form>\
<button type=\"submit\" name=\"offset\" value=\"-1\"> BACKWARD </button>\
<button type=\"submit\" name=\"offset\" value=\"0\"> RST </button>\
<button type=\"submit\" name=\"offset\" value=\"1\"> FORWARD </button> <br>\
Current SP: %f <br>\
Relative SP: %f <br>\
</form>\
<p><a href=http://192.168.4.1/control_test1> PID Tuning.</a>\
</body></html>"
// the names in these parse templates have to match the names
// in the <form> above: used to find data in the 'query string'
// part of the URL requested by the browser
// <p><input type=\"submit\">\
#define BASE_DESIRED_SP float2fix15(2.70f)
// PID Controller
struct PID
{
fix15 Kp; // Kristaps Porzingus
fix15 Ki; // Kyrie Irving
fix15 Kd; // kevin durant Easymoneysniper (our favorite player and PID term)
fix15 prev_err;
fix15 integral;
fix15 max_integral;
int max;
int min;
};
// Initial PID paramters
struct PID pid = {float2fix15(150.0), float2fix15(100.0), float2fix15(25.0), 0, 0, int2fix15(5000), 4000, -4000};
volatile bool motor_enabled = true; // motor off when holding button
fix15 desired_angle;
fix15 err;
volatile int target_speed_cmd = 0;
volatile int cmd_timeout_ticks = 0;
#define CMD_TIMEOUT_TICKS 300
#define MAX_GESTURE_ANGLE_DEG 5.0f
volatile fix15 gesture_angle_offset = 0;
#define LED_PARAM "led=%d"
#define TEST_FMT "test=%d"
#define TEST2_FMT "test2=%d"
#define COLOR_PARAM "color=%d"
#define KP_PARAM "kp=%f"
#define KI_PARAM "ki=%f"
#define KD_PARAM "kd=%f"
#define SP_PARAM "spoint=%f"
#define OFFSET_PARAM "offset=%d"
// web page names
#define CONTROL_TEST1 "/control_test1"
#define CONTROL_TEST2 "/control_test2"
//NEW CODE BEGINS
#define MSG_PAGE "/msg"
#define MSG_PARAM "msg=%127s"
static char last_msg[128] = {0};
//NEW CODE ENDS
//#define NEW_PAGE "/newpage"
// The LED is on cy243 gpio 0 NOT the rp2040
#define LED_GPIO 0
// the redirect using the IP adddress specified in MAIN
#define HTTP_RESPONSE_REDIRECT "HTTP/1.1 302 Redirect\nLocation: http://%s" CONTROL_TEST1 "\n\n"
// ===========================
// connect state structures
// ===========================
typedef struct TCP_SERVER_T_ {
struct tcp_pcb *server_pcb;
bool complete;
ip_addr_t gw;
async_context_t *context;
} TCP_SERVER_T;
typedef struct TCP_CONNECT_STATE_T_ {
struct tcp_pcb *pcb;
int sent_len;
char headers[512]; //128
char result[2048]; // 256
int header_len;
int result_len;
ip_addr_t *gw;
} TCP_CONNECT_STATE_T;
// =====================================
// called at end of 'tcp_server_sent'
// and several times in 'tcp_server_recv'
// and other places that open a conneciton
static err_t tcp_close_client_connection(TCP_CONNECT_STATE_T *con_state, struct tcp_pcb *client_pcb, err_t close_err) {
if (client_pcb) {
assert(con_state && con_state->pcb == client_pcb);
tcp_arg(client_pcb, NULL);
tcp_poll(client_pcb, NULL, 0);
tcp_sent(client_pcb, NULL);
tcp_recv(client_pcb, NULL);
tcp_err(client_pcb, NULL);
err_t err = tcp_close(client_pcb);
if (err != ERR_OK) {
DEBUG_printf("close failed %d, calling abort\n", err);
tcp_abort(client_pcb);
close_err = ERR_ABRT;
}
if (con_state) {
free(con_state);
}
}
return close_err;
}
// =======================================
// not called in this pgm, since code never exits
/*
static void tcp_server_close(TCP_SERVER_T *state) {
if (state->server_pcb) {
tcp_arg(state->server_pcb, NULL);
tcp_close(state->server_pcb);
state->server_pcb = NULL;
}
}
*/
// ========================================
//
static err_t tcp_server_sent(void *arg, struct tcp_pcb *pcb, u16_t len) {
TCP_CONNECT_STATE_T *con_state = (TCP_CONNECT_STATE_T*)arg;
DEBUG_printf("tcp_server_sent %u\n", len);
con_state->sent_len += len;
if (con_state->sent_len >= con_state->header_len + con_state->result_len) {
DEBUG_printf("all done\n");
return tcp_close_client_connection(con_state, pcb, ERR_OK);
}
return ERR_OK;
}
float new_desired = 0.0f;
int motor_state = 0;
float K_p = 0;
float K_i = 0;
float K_d = 0;
int change_offset = 0;
// ========================================
// This routine actially parses the GET response to figure out what
// the browwer user asked for on the web page.
// This is another part I chnaged from the example
// to support more general parsing of several data items
static int test_server_content(const char *request, const char *params, char *result, size_t max_result_len) {
int len = 0;
// three data items sent in this example, but could be many more
static char param1[16], param2[16], param3[16], param4[16], param5[16], param6[16], param7[16];
static char* token ;
// is it the ledtest page?
if (strncmp(request, CONTROL_TEST1, sizeof(CONTROL_TEST1) - 1) == 0) {
// Get the curent state of the led
// not used here, except as backup if web form fails
//bool value;
//cyw43_gpio_get(&cyw43_state, LED_GPIO, &value);
//int led_state = value;
// See what data the user asked for:
// the params string is trimmed by the receive funciton below to have
// only user-supplied data
// general data format is
// http://192.168.4.1/ledtest?name1=value1&name2=data2&name3=data3 and etc
// by the time the string gets here, the trimmed version is
// name1=value1&name2=data2&name3=data3
if (params) {
// parse the params using '&' delimeter
token = strtok((char*)params, "& ");
strcpy(param1, token) ;
token = strtok(NULL, "& ");
strcpy(param2, token) ;
token = strtok(NULL, "& ");
strcpy(param3, token) ;
token = strtok(NULL, "& ");
strcpy(param4, token) ;
token = strtok(NULL, "& ");
strcpy(param5, token) ;
// now get the actual numbers
sscanf(param1, LED_PARAM, &motor_enabled);
// force to binary value and set LED state
motor_enabled = motor_enabled > 0 ;
cyw43_gpio_set(&cyw43_state, 0, motor_enabled);
// set global to signal graphics thread
// the second and third parameter begining is delimited by an '&'
// these tell the graphics thread what numbers to draw
sscanf(param2, KP_PARAM, &K_p);
sscanf(param3, KI_PARAM, &K_i);
sscanf(param4, KD_PARAM, &K_d);
sscanf(param5, SP_PARAM, &new_desired);
pid.Kp = float2fix15(K_p);
pid.Ki = float2fix15(K_i);
pid.Kd = float2fix15(K_d);
desired_angle = float2fix15(new_desired);
}
// Generate result web page using the above data
// by filling in the format fields in the http string.
// test2_field occcurs twice: once for the slider position
// and once to update the numwerical value interactively.
len = snprintf(result, max_result_len, CONTROL_TEST1_BODY, motor_enabled? "ON":"OFF",
motor_enabled? " ":"selected", motor_enabled? "selected":" ",
motor_enabled? "LawnGreen":"black",
fix2float15(pid.Kp), fix2float15(pid.Ki), fix2float15(pid.Kd), fix2float15(desired_angle),
fix2float15(pid.Kp), fix2float15(pid.Ki), fix2float15(pid.Kd), fix2float15(desired_angle));
}
// is it the new page
// if so parse and build the page
else if (strncmp(request, CONTROL_TEST2, sizeof(CONTROL_TEST2) - 1) == 0) {
if (params) {
// parse the params using '&' delimeter
token = strtok((char*)params, "& ");
strcpy(param1, token) ;
// now get the actual numbers
sscanf(param1, OFFSET_PARAM , &change_offset);
// signal new color
if (change_offset > 0)
{
desired_angle = (desired_angle > (BASE_DESIRED_SP + int2fix15(3))) ? desired_angle : desired_angle + int2fix15(1);
}
else if (change_offset < 0)
{
desired_angle = (desired_angle < (BASE_DESIRED_SP - int2fix15(3))) ? desired_angle : desired_angle - int2fix15(1);
}
else if (change_offset == 0)
{
desired_angle = BASE_DESIRED_SP;
}
}
// construct the approximate 24 bit color from the 4 bit input 'color'
int longcolor = 0x000000 ;
// OR in the bits of each color 1-bit red, 1-bit blue, 2-bits green
int red_part = (color & 0b1000)? 0xff0000 : 0 ;
int green_part = (color & 0b0011)? 0x003f00 | ((color& 0b0011)<<14) : 0 ;
int blue_part = (color & 0b0100)? 0x0000ff : 0 ;
longcolor = red_part | green_part | blue_part ;
// build the page
len = snprintf(result, max_result_len, CONTROL_TEST2_BODY, \
fix2float15(desired_angle), fix2float15(desired_angle - BASE_DESIRED_SP));
}
else if (strncmp(request, MSG_PAGE, sizeof(MSG_PAGE) - 1) == 0) {
// e.g. /msg?msg=FORWARD or /msg?msg=BACKWARD or /msg?msg=STOP
char msg_buf[128] = {0};
if (params) {
// very simple parsing: expect "msg=<TEXT>"
sscanf(params, MSG_PARAM, msg_buf);
}
// Store and print the received message
strncpy(last_msg, msg_buf, sizeof(last_msg) - 1);
last_msg[sizeof(last_msg) - 1] = '\0';
printf("Received message from client: '%s'\n", last_msg);
int recognized = 0;
if (strcmp(last_msg, "FORWARD") == 0) {
target_speed_cmd = +1;
recognized = 1;
}
else if (strcmp(last_msg, "BACKWARD") == 0) {
target_speed_cmd = -1;
recognized = 1;
}
else if (strcmp(last_msg, "STOP") == 0) {
target_speed_cmd = 0;
recognized = 1;
}
if (recognized) {
// Reset timeout counter whenever we get a valid command
cmd_timeout_ticks = 0;
// Map command to an extra tilt angle
float angle_deg = 0.0f;
if (target_speed_cmd > 0) angle_deg = MAX_GESTURE_ANGLE_DEG;
else if (target_speed_cmd < 0) angle_deg = -MAX_GESTURE_ANGLE_DEG;
else angle_deg = 0.0f;
gesture_angle_offset = float2fix15(angle_deg);
}
// Tiny response to keep the HTTP client happy
len = snprintf(result, max_result_len,
"OK %s", last_msg);
}
return len;
}
// ========================================
// receives brwoser url request, parses out the query string
// to send to the funciton just above, does some error checking
// then sends the html back to the browser
err_t tcp_server_recv(void *arg, struct tcp_pcb *pcb, struct pbuf *p, err_t err) {
TCP_CONNECT_STATE_T *con_state = (TCP_CONNECT_STATE_T*)arg;
if (!p) {
DEBUG_printf("connection closed\n");
return tcp_close_client_connection(con_state, pcb, ERR_OK);
}
assert(con_state && con_state->pcb == pcb);
if (p->tot_len > 0) {
DEBUG_printf("tcp_server_recv %d err %d\n", p->tot_len, err);
#if 0
for (struct pbuf *q = p; q != NULL; q = q->next) {
DEBUG_printf("in: %.*s\n", q->len, q->payload);
}
#endif
// Copy the request into the buffer
pbuf_copy_partial(p, con_state->headers, p->tot_len > sizeof(con_state->headers) - 1 ? sizeof(con_state->headers) - 1 : p->tot_len, 0);
// Handle GET request
if (strncmp(HTTP_GET, con_state->headers, sizeof(HTTP_GET) - 1) == 0) {
char *request = con_state->headers + sizeof(HTTP_GET); // + space
char *result = con_state->result ;
// find the params start in the request (indicated by '?')
char *params = strchr(request, '?');
// find the param end (i think) and force a null
if (params) {
if (*params) {
char *space = strchr(request, ' ');
*params++ = 0;
if (space) {
*space = 0;
}
} else {
params = NULL;
}
}
// Generate content html
con_state->result_len = test_server_content(request, params, con_state->result, sizeof(con_state->result));
DEBUG_printf("Request: %s?%s\n", request, params);
DEBUG_printf("Result: %d\n", con_state->result_len);
// Check we had enough buffer space
if (con_state->result_len > sizeof(con_state->result) - 1) {
DEBUG_printf("Too much result data %d\n", con_state->result_len);
return tcp_close_client_connection(con_state, pcb, ERR_CLSD);
}
// Generate web page header
if (con_state->result_len > 0) {
con_state->header_len = snprintf(con_state->headers, sizeof(con_state->headers), HTTP_RESPONSE_HEADERS,
200, con_state->result_len);
if (con_state->header_len > sizeof(con_state->headers) - 1) {
DEBUG_printf("Too much header data %d\n", con_state->header_len);
return tcp_close_client_connection(con_state, pcb, ERR_CLSD);
}
} else {
// Send redirect
con_state->header_len = snprintf(con_state->headers, sizeof(con_state->headers), HTTP_RESPONSE_REDIRECT,
ipaddr_ntoa(con_state->gw));
DEBUG_printf("Sending redirect %s", con_state->headers);
}
// Send the headers to the client
con_state->sent_len = 0;
err_t err = tcp_write(pcb, con_state->headers, con_state->header_len, 0);
if (err != ERR_OK) {
DEBUG_printf("failed to write header data %d\n", err);
return tcp_close_client_connection(con_state, pcb, err);
}
// Send the body to the client
if (con_state->result_len) {
err = tcp_write(pcb, con_state->result, con_state->result_len, 0);
if (err != ERR_OK) {
DEBUG_printf("failed to write result data %d\n", err);
return tcp_close_client_connection(con_state, pcb, err);
}
}
}
tcp_recved(pcb, p->tot_len);
}
pbuf_free(p);
return ERR_OK;
}
// ========================================
//
static err_t tcp_server_poll(void *arg, struct tcp_pcb *pcb) {
TCP_CONNECT_STATE_T *con_state = (TCP_CONNECT_STATE_T*)arg;
DEBUG_printf("tcp_server_poll_fn\n");
return tcp_close_client_connection(con_state, pcb, ERR_OK); // Just disconnect clent?
}
// ========================================
//
static void tcp_server_err(void *arg, err_t err) {
TCP_CONNECT_STATE_T *con_state = (TCP_CONNECT_STATE_T*)arg;
if (err != ERR_ABRT) {
DEBUG_printf("tcp_client_err_fn %d\n", err);
tcp_close_client_connection(con_state, con_state->pcb, err);
}
}
// ========================================
//
static err_t tcp_server_accept(void *arg, struct tcp_pcb *client_pcb, err_t err) {
TCP_SERVER_T *state = (TCP_SERVER_T*)arg;
if (err != ERR_OK || client_pcb == NULL) {
DEBUG_printf("failure in accept\n");
return ERR_VAL;
}
DEBUG_printf("client connected\n");
// Create the state for the connection
TCP_CONNECT_STATE_T *con_state = calloc(1, sizeof(TCP_CONNECT_STATE_T));
if (!con_state) {
DEBUG_printf("failed to allocate connect state\n");
return ERR_MEM;
}
con_state->pcb = client_pcb; // for checking
con_state->gw = &state->gw;
// setup connection to client
tcp_arg(client_pcb, con_state);
tcp_sent(client_pcb, tcp_server_sent);
tcp_recv(client_pcb, tcp_server_recv);
tcp_poll(client_pcb, tcp_server_poll, POLL_TIME_S * 2);
tcp_err(client_pcb, tcp_server_err);
return ERR_OK;
}
// =======================================
// called from MAIN to start tcp server
// =======================================
static bool tcp_server_open(void *arg, const char *ap_name) {
TCP_SERVER_T *state = (TCP_SERVER_T*)arg;
DEBUG_printf("starting server on port %d\n", TCP_PORT);
struct tcp_pcb *pcb = tcp_new_ip_type(IPADDR_TYPE_ANY);
if (!pcb) {
DEBUG_printf("failed to create pcb\n");
return false;
}
err_t err = tcp_bind(pcb, IP_ANY_TYPE, TCP_PORT);
if (err) {
DEBUG_printf("failed to bind to port %d\n",TCP_PORT);
return false;
}
state->server_pcb = tcp_listen_with_backlog(pcb, 1);
if (!state->server_pcb) {
DEBUG_printf("failed to listen\n");
if (pcb) {
tcp_close(pcb);
}
return false;
}
tcp_arg(state->server_pcb, state);
tcp_accept(state->server_pcb, tcp_server_accept);
printf("Try connecting to '%s' with password 'password'\n", ap_name);
return true;
}
//=========================================
// never truning off wifi in my code
/*
// This "worker" function is called to safely perform work when instructed by key_pressed_func
void key_pressed_worker_func(async_context_t *context, async_when_pending_worker_t *worker) {
//>>>>assert(worker->user_data);
printf("Disabling wifi\n");
cyw43_arch_disable_ap_mode();
//>>>>((TCP_SERVER_T*)(worker->user_data))->complete = true;
}
*/
/*
static async_when_pending_worker_t key_pressed_worker = {
.do_work = key_pressed_worker_func
};
*/
void key_pressed_func(void *param) {
assert(param);
int key = getchar_timeout_us(0); // get any pending key press but don't wait
if (key == 'd' || key == 'D') {
// We are probably in irq context so call wifi in a "worker"
//>>>> async_context_set_work_pending(((TCP_SERVER_T*)param)->context, &key_pressed_worker);
}
}
// ==================================================
// access thread protothread
// ==================================================
static PT_THREAD (protothread_access(struct pt *pt)){
PT_BEGIN(pt);
//if (!state)
// return;
while(true) {
//
PT_YIELD_usec(100000);
}
PT_END(pt);
}
// ==================================================
// gpio2 intensity
// this is really just a test of multitasking
// compatability with LWIP
// ==================================================
static PT_THREAD (protothread_toggle_gpio6(struct pt *pt))
{
PT_BEGIN(pt);
//
// data structure for interval timer
PT_INTERVAL_INIT() ;
// set up LED gpio 2
gpio_init(6) ;
gpio_set_dir(6, GPIO_OUT) ;
gpio_put(6, true);
while(1) {
// cheesy PWM
PT_YIELD_INTERVAL(10000) ;
//
gpio_put(6, true);
// intensity % range 0-99 works
PT_YIELD_usec(test2_field*100);
gpio_put(6, false);
// NEVER exit while
} // END WHILE(1)
PT_END(pt);
} // blink thread
// END SERVER CODE
// PID CODE
#define HB_FWD_PIN 14
#define HB_BCK_PIN 15
// Arrays in which raw measurements will be stored
fix15 acceleration[3], gyro[3];
fix15 accel_angle, gyro_angle_delta;
fix15 complementary_angle;
fix15 filter_acc_z, filter_acc_y;
// character array
char screentext[40];
// draw speed
int threshold = 50 ;
// Some macros for max/min/abs
#define min(a,b) ((a<b) ? a:b)
#define max(a,b) ((a<b) ? b:a)
#define abs(a) ((a>0) ? a:-a)
// semaphore
static struct pt_sem vga_semaphore ;
// Some paramters for PWM
#define WRAPVAL 5000
#define CLKDIV 25.0
#define PWM_OUT_F 4
#define PWM_OUT_B 5
#define PI float2fix15(3.14159265358979)
uint slice_num;
// BUTTON
#define BUTTON_PIN 15
#define DEBOUNCE_SAMPLES 4
static struct pt_sem sequence_semaphore;
volatile bool sequence_running = false; // ignore re-triggers while running
// helper for degrees → fix15
static inline fix15 deg15(float deg) { return float2fix15(deg); }
// PWM duty cycle
volatile int control ;
volatile int old_control ;
fix15 control_pix;
// Combines gyro and accel data to allow stabl angle estimation
void complementary_filter()
{
filter_acc_y = filter_acc_y + ((acceleration[1] - filter_acc_y)>>4);
filter_acc_z = filter_acc_z + ((acceleration[2] - filter_acc_z)>>4);
// accel_angle = multfix15(divfix(acceleration[0], acceleration[1]), oneeightyoverpi);
accel_angle = multfix15(float2fix15(atan2(-filter_acc_y, filter_acc_z) ), oneeightyoverpi);
gyro_angle_delta = multfix15(gyro[0], zeropt001) ;
complementary_angle = multfix15(complementary_angle - gyro_angle_delta, zeropt999) + multfix15(accel_angle, zeropt001);
}
fix15 secondary_kp = float2fix15(100.0);
// Perform one PID step
// Perform one PID step
void pid_step()
{
// Use base desired angle plus gesture-driven offset
fix15 effective_desired = desired_angle + gesture_angle_offset;
// PID error
err = (effective_desired - complementary_angle);
// Integral with clamping and non-negativity
pid.integral = (pid.integral + err) < pid.max_integral ?
(pid.integral + err) : pid.max_integral; // clamp integral
pid.integral = pid.integral > 0 ? pid.integral : 0;
pid.prev_err = err;
// Derivative term from gyro
fix15 derivative_term = multfix15(gyro[0], pid.Kd);
int temp_control = fix2int15(
multfix15(err, pid.Kp) +
multfix15(pid.integral, pid.Ki) +
derivative_term
);
int kp_offset = multfix15(secondary_kp, err);
// Clamp overall control
if (temp_control > pid.max) {
control = pid.max;
}
else if (temp_control < pid.min) {
control = pid.min;
}
else {
control = temp_control;
}
// Safety: disable motor if requested
if (!motor_enabled) control = 0;
// Only touch PWM when control actually changes
if (control != old_control) {
old_control = control;
if (control < 0) {
pwm_set_chan_level(slice_num, PWM_CHAN_A, 0);
pwm_set_chan_level(slice_num, PWM_CHAN_B, abs(control) + 1000 + kp_offset);
}
else {
pwm_set_chan_level(slice_num, PWM_CHAN_B, 0);
pwm_set_chan_level(slice_num, PWM_CHAN_A, abs(control) + 1000);
}
}
}
// Reads IMU, updates filter and PID, then signals VGA
// Interrupt service routine
void on_pwm_wrap() {
// Clear the interrupt flag that brought us here
pwm_clear_irq(pwm_gpio_to_slice_num(5));
// === Gesture command timeout ===
// This ISR runs at ~1 kHz with the current PWM settings.
// If we haven't seen a FORWARD/BACKWARD/STOP in CMD_TIMEOUT_TICKS calls,
// force the robot to STOP (no extra tilt).
if (cmd_timeout_ticks < CMD_TIMEOUT_TICKS + 1) {
cmd_timeout_ticks++;
}
if (cmd_timeout_ticks > CMD_TIMEOUT_TICKS) {
target_speed_cmd = 0;
gesture_angle_offset = 0;
}
// Read the IMU
mpu6050_read_raw(acceleration, gyro);
// Update complementary filter
complementary_filter();
// Run PID with (desired_angle + gesture_angle_offset)
pid_step();
// Signal VGA to draw
PT_SEM_SIGNAL(pt, &vga_semaphore);
}
// Plots angles and control signals on the VGA display
// Thread that draws to VGA display
static PT_THREAD (protothread_vga(struct pt *pt))
{
// Indicate start of thread
PT_BEGIN(pt) ;
// We will start drawing at column 81
static int xcoord = 81 ;
// Rescale the measurements for display
static float OldRange = 180. ; // (+/- 90)
static float NewRange = 150. ; // (looks nice on VGA)
static float OldMin = -90. ;
static float OldMax = 90. ;
// Control rate of drawing
static int throttle ;
// Draw the static aspects of the display
setTextSize(1) ;
setTextColor(WHITE);
// Draw bottom plot
drawHLine(75, 430, 5, CYAN) ;
drawHLine(75, 355, 5, CYAN) ;
drawHLine(75, 280, 5, CYAN) ;
drawVLine(80, 280, 150, CYAN) ;
sprintf(screentext, "+1400") ;
setCursor(50, 350) ;
writeString(screentext) ;
sprintf(screentext, "+2800") ;
setCursor(50, 280) ;
writeString(screentext) ;
sprintf(screentext, "0") ;
setCursor(50, 425) ;
writeString(screentext) ;
// Draw top plot
drawHLine(75, 230, 5, CYAN) ;
drawHLine(75, 155, 5, CYAN) ;
drawHLine(75, 80, 5, CYAN) ;
drawHLine(75, 180, 5, CYAN) ;
drawHLine(75, 130, 5, CYAN) ;
drawVLine(80, 80, 150, CYAN) ;
// Prints the angles
sprintf(screentext, "0") ;
setCursor(50, 150) ;
writeString(screentext) ;
sprintf(screentext, "+30") ;
setCursor(50, 125) ;
writeString(screentext) ;
sprintf(screentext, "-30") ;
setCursor(50, 175) ;
writeString(screentext) ;
sprintf(screentext, "+90") ;
setCursor(45, 75) ;
writeString(screentext) ;
sprintf(screentext, "-90") ;
setCursor(45, 225) ;
writeString(screentext) ;
while (true) {
// Wait on semaphore
PT_SEM_WAIT(pt, &vga_semaphore);
// Increment drawspeed controller
throttle += 1 ;
// If the controller has exceeded a threshold, draw
if (throttle >= threshold) {
// Zero drawspeed controller
throttle = 0 ;
// Erase a column
drawVLine(xcoord, 0, 480, BLACK) ;
// Draw bottom plot (multiply by 120 to scale from +/-2 to +/-250)
// drawPixel(xcoord, 430 - (int)(NewRange*((float)((fix2float15(acceleration[0])*120.0)-OldMin)/OldRange)), WHITE) ;
// drawPixel(xcoord, 430 - (int)(NewRange*((float)((fix2float15(acceleration[1])*120.0)-OldMin)/OldRange)), RED) ;
// drawPixel(xcoord, 430 - (int)(NewRange*((float)((fix2float15(acceleration[2])*120.0)-OldMin)/OldRange)), GREEN) ;
control_pix = control_pix + ((int2fix15(control) - control_pix) >> 5);
drawPixel(xcoord, 505 - (int)(NewRange*((float)((fix2float15(control_pix) / 15.55)-OldMin)/OldRange)), GREEN) ;
// Draw top plot
// drawPixel(xcoord, 230 - (int)(NewRange*((float)((fix2float15(gyro[0]))-OldMin)/OldRange)), WHITE) ;
// drawPixel(xcoord, 230 - (int)(NewRange*((float)((fix2float15(gyro[1]))-OldMin)/OldRange)), RED) ;
drawPixel(xcoord, 230 - (int)(NewRange*((float)((fix2float15(desired_angle)) - OldMin)/OldRange)), GREEN) ;
drawPixel(xcoord, 230 - (int)(NewRange*((float)((fix2float15(complementary_angle))-OldMin)/OldRange)), BLUE) ;
// Update horizontal cursor
if (xcoord < 609) {
xcoord += 1 ;
}
else {
xcoord = 81 ;
}
fillRect(10, 445, 620, 30, BLACK);
setCursor(10, 460);
setTextColor(WHITE);
sprintf(screentext, "PID: Kp=%5.1f Ki=%5.2f Kd=%5.1f",
fix2float15(pid.Kp), fix2float15(pid.Ki), fix2float15(pid.Kd));
writeString(screentext);
}
}
// Indicate end of thread
PT_END(pt);
}
// User input thread. User can change draw speed
static PT_THREAD (protothread_serial(struct pt *pt))
{
PT_BEGIN(pt) ;
static char classifier = 0;
static int test_in ;
static float float_in ;
while(1) {
printf("Complementary Angle: %f \n", fix2float15(complementary_angle));
printf("error: %f \n", fix2float15(desired_angle - complementary_angle));
// printf("control: %d \n", (control));
// ################## BEGIN AI-GENERATED CODE ##################
// printf("enter p/i/d/a: ");
// serial_read;
// sscanf(pt_serial_in_buffer, "%c", &classifier);
// if (classifier == 'p') {
// sprintf(pt_serial_out_buffer, "Set Kp: ");
// serial_write;
// serial_read;
// sscanf(pt_serial_in_buffer, "%f", &float_in);
// pid.Kp = float2fix15(float_in);
// }
// // ################### END AI-GENERATED CODE ###################
// // adjust Ki term in serial
// else if (classifier == 'i') {
// sprintf(pt_serial_out_buffer, "Set Ki: ");
// serial_write;
// serial_read;
// sscanf(pt_serial_in_buffer, "%f", &float_in);
// pid.Ki = float2fix15(float_in);
// }
// // adjust Kd term in serial
// else if (classifier == 'd') {
// sprintf(pt_serial_out_buffer, "Set Kd: ");
// serial_write;
// serial_read;
// sscanf(pt_serial_in_buffer, "%f", &float_in);
// pid.Kd = float2fix15(float_in);
// }
// // adjust desired_angle term in serial
// else if (classifier == 'a') {
// sprintf(pt_serial_out_buffer, "Set Desired Angle: ");
// serial_write;
// serial_read;
// sscanf(pt_serial_in_buffer, "%f", &float_in);
// desired_angle = float2fix15(float_in);
// }
}
PT_END(pt) ;
}
// Entry point for core 1
void core1_entry() {
DEBUG_printf("CORE 1 ENTRY");
////////////////////////////////////////////////////////////////////////
///////////////////////// I2C CONFIGURATION ////////////////////////////
i2c_init(I2C_CHAN, I2C_BAUD_RATE) ;
gpio_set_function(SDA_PIN, GPIO_FUNC_I2C) ;
gpio_set_function(SCL_PIN, GPIO_FUNC_I2C) ;
// Pullup resistors on breakout board, don't need to turn on internals
// gpio_pull_up(SDA_PIN) ;
// gpio_pull_up(SCL_PIN) ;
// MPU6050 initialization
mpu6050_reset();
mpu6050_read_raw(acceleration, gyro);
////////////////////////////////////////////////////////////////////////
///////////////////////// PWM CONFIGURATION ////////////////////////////
////////////////////////////////////////////////////////////////////////
// Tell GPIO PWM_OUT that it is allocated to the PWM
gpio_set_function(PWM_OUT_F, GPIO_FUNC_PWM);
gpio_set_function(PWM_OUT_B, GPIO_FUNC_PWM);
// Find out which PWM slice is connected to GPIO 5 (it's slice 2, same for 4)
slice_num = pwm_gpio_to_slice_num(PWM_OUT_F);
// Mask our slice's IRQ output into the PWM block's single interrupt line,
// and register our interrupt handler
pwm_clear_irq(slice_num);
pwm_set_irq_enabled(slice_num, true);
irq_set_exclusive_handler(PWM_IRQ_WRAP, on_pwm_wrap);
irq_set_enabled(PWM_IRQ_WRAP, true);
// This section configures the period of the PWM signals
pwm_set_wrap(slice_num, WRAPVAL) ;
pwm_set_clkdiv(slice_num, CLKDIV) ;
// This sets duty cycle
pwm_set_chan_level(slice_num, PWM_CHAN_B, 0);
pwm_set_chan_level(slice_num, PWM_CHAN_A, 0);
// Start the channel
pwm_set_mask_enabled((1u << slice_num));
////////////////////////////////////////////////////////////////////////
////////////////////// H-BRIDGE CONFIGURATION //////////////////////////
////////////////////////////////////////////////////////////////////////
gpio_set_dir(HB_FWD_PIN, GPIO_OUT);
gpio_set_dir(HB_BCK_PIN, GPIO_OUT);
desired_angle = float2fix15(2.85f);
// start core 1
gpio_put(HB_FWD_PIN, true);
pt_schedule_start ;
}
int main() {
// semaphores
PT_SEM_INIT(&vga_semaphore, 0);
PT_SEM_INIT(&sequence_semaphore, 0);
// Overclock
set_sys_clock_khz(150000, true) ;
// Initialize stdio
stdio_init_all();
// Initialize VGA
initVGA() ;
DEBUG_printf("STARTING UP...");
// =====================================
// turn on ADC (move to thread)
// adc_init();
// adc_set_temp_sensor_enabled(true);
// adc_select_input(4);
//========================================
// network init
// start the cyw43 in access point mode
// start the DHCP server
// start the DNS server
// start the TCP server
// =======================================
TCP_SERVER_T *state = calloc(1, sizeof(TCP_SERVER_T));
if (!state) {
DEBUG_printf("failed to allocate state\n");
return 1;
}
DEBUG_printf("After state check");
if (cyw43_arch_init()) {
DEBUG_printf("failed to initialise\n");
return 1;
}
DEBUG_printf("After cyw43_arch_init");
// Get notified if the user presses a key
state->context = cyw43_arch_async_context();
//>>>> key_pressed_worker.user_data = state;
//>>>>async_context_add_when_pending_worker(cyw43_arch_async_context(), &key_pressed_worker);
stdio_set_chars_available_callback(key_pressed_func, state);
// access point SSID and PASSWORD
// WPA2 authorization
const char *ap_name = "jamayne's wifi";
#if 1
const char *password = "password";
#else
const char *password = NULL;
#endif
cyw43_arch_enable_ap_mode(ap_name, password, CYW43_AUTH_WPA2_AES_PSK);
// 'state' is a pointer to type TCP_SERVER_T
ip4_addr_t mask;
IP4_ADDR(ip_2_ip4(&state->gw), 192, 168, 4, 1);
IP4_ADDR(ip_2_ip4(&mask), 255, 255, 255, 0);
// Start the dhcp server
// and set picoW IP address from 'state' structure
// set 'mask' as defined above
dhcp_server_t dhcp_server;
dhcp_server_init(&dhcp_server, &state->gw, &mask);
// Start the dns server
// and set picoW IP address from 'state' structure
dns_server_t dns_server;
dns_server_init(&dns_server, &state->gw);
DEBUG_printf("Trying to open tcp server");
if (!tcp_server_open(state, ap_name)) {
DEBUG_printf("failed to open server\n");
return 1;
}
////////////////////////////////////////////////////////////////////////
///////////////////////////// ROCK AND ROLL ////////////////////////////
////////////////////////////////////////////////////////////////////////
// start core 1
multicore_reset_core1();
multicore_launch_core1(core1_entry);
pt_add_thread(protothread_toggle_gpio6) ;
pt_add_thread(protothread_serial) ;
pt_schedule_start ;
dns_server_deinit(&dns_server);
dhcp_server_deinit(&dhcp_server);
cyw43_arch_deinit();
}
