Penny the Plotter - Final Report

/**
 * Copyright (c) 2022 Andrew McDonnell
 *
 * SPDX-License-Identifier: BSD-3-Clause
 */

// Standard libraries
#include <string.h>
#include <stdlib.h>
#include <stdbool.h>
#include <pico/multicore.h>
#include "hardware/sync.h"
#include "hardware/gpio.h"
#include "hardware/timer.h"
#include "hardware/uart.h"
#include "hardware/pwm.h"
#include "stdio.h"

#include "pico/stdlib.h"
#include "pico/cyw43_arch.h"

#include "lwip/pbuf.h"
#include "lwip/udp.h"
#include "lwip/opt.h"
#include "lwip/debug.h"
#include "lwip/stats.h"
#include "lwip/dns.h"
#include "lwip/netif.h"
#include "pt_cornell_rp2040_v1_4.h"

// udp constants
#define UDP_PORT 5555
#define UDP_MSG_LEN_MAX 64
#define UDP_TARGET "-----" // laptop address
#define UDP_INTERVAL_MS 10

// Module A 
#define EN_A1   10
#define IN1_A1  11
#define IN2_A1  12

#define EN_A2   13
#define IN1_A2  14
#define IN2_A2  15

// Module B 
#define EN_B1   4   
#define IN1_B1  19
#define IN2_B1  18

#define EN_B2   22
#define IN1_B2  8
#define IN2_B2  9

// PWM duty cycle max
#define MAX_DUTY 500

// ENCODER DEFINES 
#define ENC_A1_A    6
#define ENC_A1_B    7

#define ENC_A2_A    16
#define ENC_A2_B    17

#define ENC_B1_A    2
#define ENC_B1_B    3

#define ENC_B2_A    20
#define ENC_B2_B    21

// Encoder global state
volatile int32_t enc_count_a1 = 0;
volatile int32_t enc_count_a2 = 0;
volatile int32_t enc_count_b1 = 0;
volatile int32_t enc_count_b2 = 0;

// Motor speed setpoints 
volatile int mA1_sp = 0;
volatile int mA2_sp = 0;
volatile int mB1_sp = 0;
volatile int mB2_sp = 0;

// PWM duties applied to motors
static int duty_A1 = 0;
static int duty_A2 = 0;
static int duty_B1 = 0;
static int duty_B2 = 0;

// Simple proportional gains
#define MOTOR_KP_NUM 1
#define MOTOR_KP_DEN 4   // Kp = 0.25

// Motor Control Helper Functions
void pwm_setup(uint pin) {
    gpio_set_function(pin, GPIO_FUNC_PWM);
    uint slice = pwm_gpio_to_slice_num(pin);
    pwm_set_wrap(slice, MAX_DUTY);
    pwm_set_clkdiv(slice, 100.0f);   // ~1 kHz PWM
    pwm_set_enabled(slice, true);
}

void motor_set(int in1, int in2, int en, int duty) {
    gpio_put(in1, duty > 0);
    gpio_put(in2, duty < 0);
    uint slice = pwm_gpio_to_slice_num(en);
    uint level = abs(duty);
    if (level > MAX_DUTY) level = MAX_DUTY;
    pwm_set_chan_level(slice, pwm_gpio_to_channel(en), level);
}

void stop_motor(int in1, int in2, int en) {
    gpio_put(in1, 0);
    gpio_put(in2, 0);
    uint slice = pwm_gpio_to_slice_num(en);
    pwm_set_chan_level(slice, pwm_gpio_to_channel(en), 0);
}


// Encoder Quadrature Decoding (ISR)
void encoder_isr(uint gpio, uint32_t events) {
    bool a_state, b_state;
    
    if (gpio == ENC_A1_A) {
        a_state = gpio_get(ENC_A1_A);
        b_state = gpio_get(ENC_A1_B);
        if (a_state == b_state) enc_count_a1++;
        else                    enc_count_a1--;
    }
    else if (gpio == ENC_A2_A) {
        a_state = gpio_get(ENC_A2_A);
        b_state = gpio_get(ENC_A2_B);
        if (a_state == b_state) enc_count_a2++;
        else                    enc_count_a2--;
    }
    else if (gpio == ENC_B1_A) {
        a_state = gpio_get(ENC_B1_A);
        b_state = gpio_get(ENC_B1_B);
        if (a_state == b_state) enc_count_b1++;
        else                    enc_count_b1--;
    }
    else if (gpio == ENC_B2_A) {
        a_state = gpio_get(ENC_B2_A);
        b_state = gpio_get(ENC_B2_B);
        if (a_state == b_state) enc_count_b2++;
        else                    enc_count_b2--;
    }
}


// Encoder Initialization
void encoder_init() {
    int enc_a_pins[] = {ENC_A1_A, ENC_A2_A, ENC_B1_A, ENC_B2_A};
    int enc_b_pins[] = {ENC_A1_B, ENC_A2_B, ENC_B1_B, ENC_B2_B};
    
    for (int i = 0; i < 4; i++) {
        // Channel A - triggers interrupt
        gpio_init(enc_a_pins[i]);
        gpio_set_dir(enc_a_pins[i], GPIO_IN);
        gpio_pull_up(enc_a_pins[i]);
        
        // Channel B - read for direction
        gpio_init(enc_b_pins[i]);
        gpio_set_dir(enc_b_pins[i], GPIO_IN);
        gpio_pull_up(enc_b_pins[i]);
    }
    
    // Shared callback for all A channels
    gpio_set_irq_enabled_with_callback(
        ENC_A1_A,
        GPIO_IRQ_EDGE_RISE | GPIO_IRQ_EDGE_FALL,
        true,
        &encoder_isr
    );
    
    gpio_set_irq_enabled(ENC_A2_A, GPIO_IRQ_EDGE_RISE | GPIO_IRQ_EDGE_FALL, true);
    gpio_set_irq_enabled(ENC_B1_A, GPIO_IRQ_EDGE_RISE | GPIO_IRQ_EDGE_FALL, true);
    gpio_set_irq_enabled(ENC_B2_A, GPIO_IRQ_EDGE_RISE | GPIO_IRQ_EDGE_FALL, true);
}

// MODULE A (Left)
void moduleA_motor1(int duty) { motor_set(IN1_A1, IN2_A1, EN_A1, duty); }
void moduleA_motor2(int duty) { motor_set(IN1_A2, IN2_A2, EN_A2, duty); }

void moduleA_stop() {
    stop_motor(IN1_A1, IN2_A1, EN_A1);
    stop_motor(IN1_A2, IN2_A2, EN_A2);
}

// MODULE B (Right) 
void moduleB_motor1(int duty) { motor_set(IN1_B1, IN2_B1, EN_B1, duty); }
void moduleB_motor2(int duty) { motor_set(IN1_B2, IN2_B2, EN_B2, duty); }

void moduleB_stop() {
    stop_motor(IN1_B1, IN2_B1, EN_B1);
    stop_motor(IN1_B2, IN2_B2, EN_B2);
}


volatile int linear_cmd  = 0;
volatile int angular_cmd = 0;

void update_motor_setpoints_from_cmd(void) {
    int wheel = linear_cmd;
    int yaw   = angular_cmd;
    mA1_sp = yaw + wheel;
    mA2_sp = yaw - wheel;
    mB1_sp = yaw + wheel;
    mB2_sp = yaw - wheel;
}

// Drive Helper
void robot_drive(int linear, int angular) {
    if (linear >  MAX_DUTY) linear =  MAX_DUTY;
    if (linear < -MAX_DUTY) linear = -MAX_DUTY;
    if (angular >  MAX_DUTY) angular =  MAX_DUTY;
    if (angular < -MAX_DUTY) angular = -MAX_DUTY;

    linear_cmd  = linear;
    angular_cmd = angular;
    update_motor_setpoints_from_cmd();
}

void robot_forward(int speed)  { robot_drive(+speed, 0); }
void robot_backward(int speed) { robot_drive(-speed, 0); }
void robot_rotate_left(int yaw){ robot_drive(0, +yaw); }
void robot_rotate_right(int yaw){ robot_drive(0, -yaw); }

void robot_stop() {
    robot_drive(0, 0);
    duty_A1 = duty_A2 = duty_B1 = duty_B2 = 0;
    moduleA_stop();
    moduleB_stop();
}

// WIFI 
#define WIFI_SSID "---"
#define WIFI_PASSWORD "---"

char recv_data[UDP_MSG_LEN_MAX];

volatile uint32_t packets_received = 0;
volatile uint32_t packets_sent = 0;
volatile uint32_t parse_errors = 0;
volatile uint32_t last_recv_time_ms = 0;

struct pt_sem new_udp_recv_s, new_udp_send_s ;

#if LWIP_UDP

static struct udp_pcb *udpecho_raw_pcb;
struct pbuf *p ;

static void
udpecho_raw_recv(void *arg, struct udp_pcb *upcb, struct pbuf *p,
                 const ip_addr_t *addr, u16_t port)
{
  LWIP_UNUSED_ARG(arg);

  if (p != NULL) {
    memcpy(recv_data, p->payload, UDP_MSG_LEN_MAX);
    PT_SEM_SIGNAL(pt, &new_udp_recv_s) ;
    pbuf_free(p);
  }
  else printf("NULL pt in callback");
}

void udpecho_raw_init(void)
{
  udpecho_raw_pcb = udp_new_ip_type(IPADDR_TYPE_ANY);
  p = pbuf_alloc(PBUF_TRANSPORT, UDP_MSG_LEN_MAX+1, PBUF_RAM);

  if (udpecho_raw_pcb != NULL) {
    err_t err;
    err = udp_bind(udpecho_raw_pcb, netif_ip4_addr(netif_list), UDP_PORT); //DHCP addr

    if (err == ERR_OK) {
      udp_recv(udpecho_raw_pcb, udpecho_raw_recv, NULL);
    } else {
      printf("bind error");
    }
  } else {
    printf("udpecho_raw_pcb error");
  }
}

#endif /* LWIP_UDP */

// UDP Send Thread
static PT_THREAD (protothread_udp_send(struct pt *pt))
{
    PT_BEGIN(pt);
    static struct udp_pcb* pcb;
    pcb = udp_new();
    pcb->remote_port = UDP_PORT ;
    pcb->local_port = UDP_PORT ;

    static ip_addr_t addr;
    ipaddr_aton(UDP_TARGET, &addr);
    
    while (true) {
        PT_SEM_WAIT(pt, &new_udp_send_s) ;

        struct pbuf *p = pbuf_alloc(PBUF_TRANSPORT, UDP_MSG_LEN_MAX+1, PBUF_RAM);
        char *req = (char *)p->payload;
        memset(req, 0, UDP_MSG_LEN_MAX+1);
        
        uint32_t uptime_ms = to_ms_since_boot(get_absolute_time());
        sprintf(req, "%lu|%d,%d|rx:%lu,err:%lu",
                packets_sent,
                linear_cmd, angular_cmd,
                packets_received,
                parse_errors);
        
        err_t er = udp_sendto(pcb, p, &addr, UDP_PORT);
        pbuf_free(p);
        if (er != ERR_OK) {
            printf("UDP send error=%d\n", er);
        } else {
            packets_sent++;
        }

#if PICO_CYW43_ARCH_POLL
        cyw43_arch_poll();
        sleep_ms(BEACON_INTERVAL_MS);
#else
        PT_YIELD_usec(UDP_INTERVAL_MS * 1000);
#endif
    }

    PT_END(pt);
}


// Thread for UDP to parse motor commands
static PT_THREAD (protothread_udp_recv(struct pt *pt))
{
    PT_BEGIN(pt);
    PT_INTERVAL_INIT();

    while(1) {
        PT_SEM_WAIT(pt, &new_udp_recv_s) ;
        
        packets_received++;
        last_recv_time_ms = to_ms_since_boot(get_absolute_time());

        int new_linear = 0, new_angular = 0;
        int parsed = sscanf(recv_data, "%d %d", &new_linear, &new_angular);
        
        if (parsed == 2) {
            robot_drive(new_linear, new_angular);
            printf("[RX] lin=%d ang=%d\n", linear_cmd, angular_cmd);
        } else {
            parse_errors++;
            printf("[ERR] Bad packet: '%s' (parsed %d)\n", recv_data, parsed);
        }
        
        PT_SEM_SIGNAL(pt, &new_udp_send_s) ;
        PT_YIELD_INTERVAL(10);
    }
    PT_END(pt);
}

// Toggle LED for UDP 
static PT_THREAD (protothread_toggle_cyw43(struct pt *pt))
{
    PT_BEGIN(pt);
    static bool LED_state = false ;
    PT_INTERVAL_INIT();

    while(1) {
        LED_state = !LED_state ;
        cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, LED_state);
        PT_YIELD_INTERVAL(500000); // 500 ms
    }
    
    PT_END(pt);
}

// helper to get sign
static inline int sgn(int x) {
    if (x > 0) return 1;
    if (x < 0) return -1;
    return 0;
}

// Proportional control relative to a target magnitude
static void equalize_to_ref(
    int *duty, int ref_mag, int actual_delta, int desired_sign)
{
    if (desired_sign == 0) {
        *duty = 0;
        return;
    }

    // initialize to a minimum value
    if (*duty == 0 && ref_mag > 0) {
        *duty = desired_sign * (ref_mag / 2);  // Start at half the reference
        return;
    }

    int actual_mag = abs(actual_delta);
    int error      = ref_mag - actual_mag;
    int delta      = (error * MOTOR_KP_NUM) / MOTOR_KP_DEN;

    int new_mag = abs(*duty) + delta;
    
    // Rate limiting
    int max_change = 50;
    int current_mag = abs(*duty);
    if (new_mag > current_mag + max_change) {
        new_mag = current_mag + max_change;
    } else if (new_mag < current_mag - max_change) {
        new_mag = current_mag - max_change;
    }
    
    if (new_mag < 0)          new_mag = 0;
    if (new_mag > MAX_DUTY)   new_mag = MAX_DUTY;

    *duty = desired_sign * new_mag;
}

// per-motor control to track its own setpoint magnitude
static void track_own_setpoint(
    int *duty, int setpoint, int actual_delta)
{
    int sig = sgn(setpoint);
    if (sig == 0) {
        *duty = 0;
        return;
    }

    int target_mag = abs(setpoint);
    int actual_mag = abs(actual_delta);
    int error      = target_mag - actual_mag;
    int delta      = (error * MOTOR_KP_NUM) / MOTOR_KP_DEN;

    int new_mag = abs(*duty) + delta;
    
    // Rate limiting
    int max_change = 50;
    int current_mag = abs(*duty);
    if (new_mag > current_mag + max_change) {
        new_mag = current_mag + max_change;
    } else if (new_mag < current_mag - max_change) {
        new_mag = current_mag - max_change;
    }
    
    if (new_mag < 0)        new_mag = 0;
    if (new_mag > MAX_DUTY) new_mag = MAX_DUTY;

    *duty = sig * new_mag;
}

static PT_THREAD (protothread_motor_control(struct pt *pt))
{
    PT_BEGIN(pt);
    PT_INTERVAL_INIT();

    static int32_t last_a1 = 0;
    static int32_t last_a2 = 0;
    static int32_t last_b1 = 0;
    static int32_t last_b2 = 0;
    static bool initialized = false;

    while (1) {
        // Encoder counts and compute deltas
        int32_t cA1 = enc_count_a1;
        int32_t cA2 = enc_count_a2;
        int32_t cB1 = enc_count_b1;
        int32_t cB2 = enc_count_b2;

        int32_t dA1 = cA1 - last_a1;
        int32_t dA2 = cA2 - last_a2;
        int32_t dB1 = cB1 - last_b1;
        int32_t dB2 = cB2 - last_b2;

        last_a1 = cA1;
        last_a2 = cA2;
        last_b1 = cB1;
        last_b2 = cB2;

        int sigA1 = sgn(mA1_sp);
        int sigA2 = sgn(mA2_sp);
        int sigB1 = sgn(mB1_sp);
        int sigB2 = sgn(mB2_sp);

        // Case 1: robot stopped
        if (linear_cmd == 0 && angular_cmd == 0) {
            duty_A1 = duty_A2 = duty_B1 = duty_B2 = 0;
            initialized = false;
        }
        // Case 2: pure forward/backward -> equalize all motors to one reference
        else if (angular_cmd == 0 && linear_cmd != 0) {
            // Initialize duties to setpoint values
            if (!initialized) {
                duty_A1 = mA1_sp;
                duty_A2 = mA2_sp;
                duty_B1 = mB1_sp;
                duty_B2 = mB2_sp;
                initialized = true;
                printf("[CTRL] Open-loop startup: A1=%d A2=%d B1=%d B2=%d\n", 
                       duty_A1, duty_A2, duty_B1, duty_B2);
            }
            
            // Use A2 or B1 as reference
            int a2_encoder = (dA2 != 0) ? dA2 : dB2;
            int ref_mag = abs(a2_encoder);
            if (ref_mag < 1) {
                ref_mag = abs(dB1);  
                if (ref_mag < 1) {
                    ref_mag = abs(mA1_sp);  
                }
            }

            // All motors equalize to the same reference magnitude
            equalize_to_ref(&duty_A1, ref_mag, dA1, sigA1);
            equalize_to_ref(&duty_A2, ref_mag, a2_encoder, sigA2);
            duty_B2 = duty_A2; 
            equalize_to_ref(&duty_B1, ref_mag, dB1, sigB1);
        }
        // Case 3: pure rotation 
        else if (linear_cmd == 0 && angular_cmd != 0) {
            if (!initialized) {
                duty_A1 = mA1_sp;
                duty_A2 = mA2_sp;
                duty_B1 = mB1_sp;
                duty_B2 = mB2_sp;
                initialized = true;
                printf("[CTRL] Open-loop startup (rotation): A1=%d A2=%d B1=%d B2=%d\n", 
                       duty_A1, duty_A2, duty_B1, duty_B2);
            }
            
            int ref_mag = abs(dA1);
            if (ref_mag < 1) ref_mag = abs(mA1_sp);

            track_own_setpoint(&duty_A1, mA1_sp, dA1);
            int a2_encoder = (dA2 != 0) ? dA2 : dB2;
            equalize_to_ref(&duty_A2, ref_mag, a2_encoder, sigA2);
            duty_B2 = duty_A2; 
            
            equalize_to_ref(&duty_B1, ref_mag, dB1, sigB1);
        }
        // Case 4: mixed arc motion 
        else {

            if (!initialized) {
                duty_A1 = mA1_sp;
                duty_A2 = mA2_sp;
                duty_B1 = mB1_sp;
                duty_B2 = mB2_sp;
                initialized = true;
                printf("[CTRL] Open-loop startup (arc): A1=%d A2=%d B1=%d B2=%d\n", 
                       duty_A1, duty_A2, duty_B1, duty_B2);
            }
            
            track_own_setpoint(&duty_A1, mA1_sp, dA1);
            int a2_encoder = (dA2 != 0) ? dA2 : dB2;
            track_own_setpoint(&duty_A2, mA2_sp, a2_encoder);
            duty_B2 = duty_A2;  
            
            track_own_setpoint(&duty_B1, mB1_sp, dB1);
        }

        // Apply updated duties 
        moduleA_motor1(duty_A1);
        moduleA_motor2(duty_A2);
        moduleB_motor1(duty_B1);
        moduleB_motor2(duty_B2);

        PT_YIELD_INTERVAL(50000); // 50 ms control loop
    }

    PT_END(pt);
}

// ====================================================
// Motor GPIO/PWM initialization
// ====================================================
void motor_init() {
    int standard_pins[] = {
        EN_A1, IN1_A1, IN2_A1,
        EN_A2, IN1_A2, IN2_A2,
        EN_B1, IN1_B1, IN2_B1,
        EN_B2, IN1_B2, IN2_B2
    };

    for (int i = 0; i < 12; i++) {
        gpio_init(standard_pins[i]);
        gpio_set_dir(standard_pins[i], GPIO_OUT);
    }

    pwm_setup(EN_A1);
    pwm_setup(EN_A2);
    pwm_setup(EN_B1);
    pwm_setup(EN_B2);
    
    // Print PWM slice info for debugging
    printf("PWM Slice Assignments:\n");
    printf("  EN_A1 (GP%d): Slice %d, Channel %c\n", EN_A1, pwm_gpio_to_slice_num(EN_A1), pwm_gpio_to_channel(EN_A1) ? 'B' : 'A');
    printf("  EN_A2 (GP%d): Slice %d, Channel %c\n", EN_A2, pwm_gpio_to_slice_num(EN_A2), pwm_gpio_to_channel(EN_A2) ? 'B' : 'A');
    printf("  EN_B1 (GP%d): Slice %d, Channel %c\n", EN_B1, pwm_gpio_to_slice_num(EN_B1), pwm_gpio_to_channel(EN_B1) ? 'B' : 'A');
    printf("  EN_B2 (GP%d): Slice %d, Channel %c\n", EN_B2, pwm_gpio_to_slice_num(EN_B2), pwm_gpio_to_channel(EN_B2) ? 'B' : 'A');

    robot_stop();
}

// ====================================================
int main() {
    stdio_init_all();

    motor_init();
    printf("Motors initialized\n");

    encoder_init();
    printf("Encoders initialized\n");

    printf("Waiting for power to stabilize...\n");
    sleep_ms(1000);

    if (cyw43_arch_init()) {
        printf("failed to initialise cyw43\n");
        return 1;
    }

    cyw43_arch_enable_sta_mode();

    int max_retries = 5;
    int retry_count = 0;
    bool connected = false;

    while (!connected && retry_count < max_retries) {
        printf("WiFi connection attempt %d/%d...\n", retry_count + 1, max_retries);
        
        if (cyw43_arch_wifi_connect_timeout_ms(
                WIFI_SSID, WIFI_PASSWORD, 
                CYW43_AUTH_WPA2_AES_PSK, 30000) == 0) {
            connected = true;
            printf("Connected: picoW IP addr: %s\n",
                   ip4addr_ntoa(netif_ip4_addr(netif_list)));
        } else {
            printf("Connection attempt %d failed, retrying...\n",
                   retry_count + 1);
            retry_count++;
            sleep_ms(2000);
        }
    }

    if (!connected) {
        printf("Failed to connect to WiFi after %d attempts\n", max_retries);
        return 1;
    }

    udpecho_raw_init();

    PT_SEM_INIT(&new_udp_send_s, 0);
    PT_SEM_INIT(&new_udp_recv_s, 0);

    pt_add_thread(protothread_udp_send);
    pt_add_thread(protothread_udp_recv);
    pt_add_thread(protothread_toggle_cyw43);
    pt_add_thread(protothread_motor_control);

    pt_schedule_start;

    cyw43_arch_deinit();
    return 0;
}

MAX_SPEED = 500


class robot_instr:
    
    def __init__(self, sender):
        self._sender = sender
    
    def _clamp(self, value, min_val=-MAX_SPEED, max_val=MAX_SPEED):
        """Clamp a value to the valid speed range."""
        return max(min_val, min(max_val, value))
    
    def _send(self, linear, angular):
        """Send a motor command."""
        linear = self._clamp(linear)
        angular = self._clamp(angular)
        self._sender.forward(f"{linear} {angular}")
    
    def move_forward(self, speed=400):
        self._send(abs(speed), 0)
    
    def move_backward(self, speed=400):
        self._send(-abs(speed), 0)
    
    def rotate_ccw(self, speed=400):
        self._send(0, abs(speed))
    
    def rotate_cw(self, speed=400):
        self._send(0, -abs(speed))
    
    def stop(self):
        """Stop all motors."""
        self._send(0, 0)
    
    def drive(self, linear, angular):
        """
        Combined linear and angular movement
        
        Args:
            linear: Linear speed (-500 to 500, positive=forward)
            angular: Angular speed (-500 to 500, positive=left)
        """
        self._send(linear, angular)
    
    def arc_forward_left(self, linear, angular):
        self._send(abs(linear), abs(angular))
    
    def arc_forward_right(self, linear, angular):
        self._send(abs(linear), -abs(angular))
    
    def arc_backward_left(self, linear, angular):
        self._send(-abs(linear), abs(angular))
    
    def arc_backward_right(self, linear, angular):
        self._send(-abs(linear), -abs(angular))

import socket
import time
from InvalidAddressError import *
from robot_instr import robot_instr


class datasender():
    def __init__(self, ip, port):
        InvalidAddressError.validate_ip(ip)
        InvalidAddressError.validate_port(port)

        # Define the IP address and port of the Pico W
        self._pico_ip = ip
        self._pico_port = port

        # Create a UDP socket
        self._sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        
        # Robot instruction interface
        self.robot = robot_instr(self)

    def forward(self, information):
        # Convert the data to bytes
        data_to_send = f"{information}".encode('utf-8')

        # Send the data to the Pico W
        self._sock.sendto(data_to_send, (self._pico_ip, self._pico_port))

        print(f"Sent: {data_to_send}")

    def close(self):
        self._sock.close()
    

if __name__ == "__main__":
    # Example usage with robot instructions
    sender = datasender("10.59.35.74", 5555)
    
    print("Testing robot commands...")
    
    move_time = time.time()
    curr_time = time.time()

    # 2 sec was a 180 degree turn
    # Test forward
    while (curr_time - move_time < 2.5):
        print("Forward...")
        sender.robot.move_forward()
        curr_time = time.time()
        time.sleep(0.5)

    stop_time = time.time()

    # Test forward
    while (curr_time - stop_time < 2):
        print("Stop...")
        sender.robot.stop()  
        curr_time = time.time()
        time.sleep(0.5)
    
    sender.close()
    print("Done!")

import turtle
import time
from datasender import datasender

# Robot configuration
ROBOT_IP = "10.59.35.74"
ROBOT_PORT = 5555
DEFAULT_SPEED = 300

# Timing configuration 
FORWARD_TIME = 1.3     
ROTATE_TIME = 0.7   
COMMAND_INTERVAL = 0.05  

# Turtle simulation scale 
TURTLE_FORWARD_DISTANCE = 100  # pixels for each forward segment

# Swerve drive parameters 
TURNING_RADIUS = 50  


def setup_turtle():
    """Initialize turtle graphics."""
    screen = turtle.Screen()
    screen.title("Penny the Plotter - Path Simulation")
    screen.bgcolor("#1a1a2e")
    
    t = turtle.Turtle()
    t.shape("turtle")
    t.color("#00d4ff")
    t.pencolor("#ff6b6b")
    t.pensize(3)
    t.speed(1)  
    
    return screen, t 


def send_command_for_duration(robot, command_func, duration, t=None, turtle_action=None):
    start_time = time.time()
    if t and turtle_action:
        action, value = turtle_action
        if action == "forward":
            steps = int(duration / COMMAND_INTERVAL)
            step_distance = value / steps if steps > 0 else value
            for _ in range(steps):
                command_func()  
                t.forward(step_distance)
                time.sleep(COMMAND_INTERVAL)
        elif action == "arc_left":
            arc_length = (value * 3.14159 * TURNING_RADIUS) / 180.0
            steps = int(duration / COMMAND_INTERVAL)
            step_angle = value / steps if steps > 0 else value
            step_distance = arc_length / steps if steps > 0 else arc_length
            for _ in range(steps):
                command_func() 
                t.forward(step_distance)
                t.left(step_angle)
                time.sleep(COMMAND_INTERVAL)
        elif action == "arc_right":
            arc_length = (value * 3.14159 * TURNING_RADIUS) / 180.0
            steps = int(duration / COMMAND_INTERVAL)
            step_angle = value / steps if steps > 0 else value
            step_distance = arc_length / steps if steps > 0 else arc_length
            for _ in range(steps):
                command_func()  
                t.forward(step_distance)
                t.right(step_angle)
                time.sleep(COMMAND_INTERVAL)
    else:
        while time.time() - start_time < duration:
            command_func()
            time.sleep(COMMAND_INTERVAL)


def draw_l_shape():
    # Setup
    screen, t = setup_turtle()
    
    print(f"Connecting to robot at {ROBOT_IP}:{ROBOT_PORT}...")
    sender = datasender(ROBOT_IP, ROBOT_PORT)
    robot = sender.robot
    
    print("\n=== Starting L-Shape Path ===\n")
    
    try:
        # Step 1: Move forward
        print("Step 1: Moving forward...")
        send_command_for_duration(robot, robot.move_forward, FORWARD_TIME, 
                                  t, ("forward", TURTLE_FORWARD_DISTANCE))
        
        # Brief stop
        print("Stopping...")
        robot.stop()
        time.sleep(0.5)
        
        # Step 2: Rotate counter-clockwise 90 degrees 
        print("Step 2: Rotating CCW 90° (curved arc)...")
        send_command_for_duration(robot, robot.rotate_ccw, ROTATE_TIME,
                                  t, ("arc_left", 90))
        
        # Brief stop
        print("Stopping...")
        robot.stop()
        time.sleep(0.5)
        
        # Step 3: Move forward again
        print("Step 3: Moving forward...")
        send_command_for_duration(robot, robot.move_forward, FORWARD_TIME,
                                  t, ("forward", TURTLE_FORWARD_DISTANCE))
        
        # Final stop
        print("Final stop...")
        robot.stop()
        
        print("\n=== Path Complete! ===")
        print("Close the turtle window to exit.")
        screen.mainloop()
        
    except KeyboardInterrupt:
        print("\nInterrupted by user")
        robot.stop()
    finally:
        sender.close()
        print("Connection closed.")


if __name__ == "__main__":
    draw_l_shape()

# == DO NOT EDIT THE FOLLOWING LINES for the Raspberry Pi Pico VS Code Extension to work ==
if(WIN32)
    set(USERHOME $ENV{USERPROFILE})
else()
    set(USERHOME $ENV{HOME})
endif()
set(sdkVersion 2.2.0)
set(toolchainVersion 14_2_Rel1)
set(picotoolVersion 2.2.0-a4)
set(picoVscode ${USERHOME}/.pico-sdk/cmake/pico-vscode.cmake)
if (EXISTS ${picoVscode})
    include(${picoVscode})
endif()
# ====================================================================================

# Use Pico W (has WiFi + lwIP)
set(PICO_BOARD pico_w CACHE STRING "Board type")

cmake_minimum_required(VERSION 3.13)

# Pull in the Pico SDK (VS Code style)
include(pico_sdk_import.cmake)

project(robot C CXX ASM)

pico_sdk_init()

add_executable(robot)

target_sources(robot PRIVATE
    robot.c
)

target_include_directories(robot PRIVATE
    ${CMAKE_CURRENT_LIST_DIR}
)

target_link_libraries(robot
    pico_cyw43_arch_lwip_threadsafe_background  # WiFi + lwIP
    pico_stdlib
    pico_bootsel_via_double_reset
    hardware_sync
    hardware_pwm
    pico_sync
)

pico_add_extra_outputs(robot)