4. EMBEDDED PROGRAMMING

from machine import Pin, ADC, I2C
import time
import ssd1306

# ---------- OLED ----------
i2c = I2C(0, scl=Pin(1), sda=Pin(0), freq=400_000)
oled = ssd1306.SSD1306_I2C(128, 64, i2c, addr=0x3C)

# ---------- Inputs ----------
joy_y = ADC(26)  # VRy -> GP26 (ADC0)
brake = Pin(15, Pin.IN, Pin.PULL_UP)  # botón a GND

# ---------- Output ----------
brake_led = Pin(16, Pin.OUT)  # LED (con resistencia) a GND

# ---------- Params ----------
MAX_KMH = 30.0
DT_MS = 50

ACCEL_KMH_PER_S = 14.0
COAST_KMH_PER_S = 6.0
BRAKE_KMH_PER_S = 30.0

DEADZONE = 0.08
SMOOTH = 0.15

speed = 0.0
y_filt = 0.5

def clamp(x, a, b):
    return a if x < a else (b if x > b else x)

while True:
    dt = DT_MS / 1000.0

    # leer joystick (0..1)
    y = joy_y.read_u16() / 65535.0
    y_filt = (1.0 - SMOOTH) * y_filt + SMOOTH * y

    # comando -1..+1 (arriba positivo)
    cmd = (y_filt - 0.5) * 2.0
    if abs(cmd) < DEADZONE:
        cmd = 0.0

    braking = (brake.value() == 0)
    brake_led.value(1 if braking else 0)

    # dinámica de velocidad
    if braking:
        speed -= BRAKE_KMH_PER_S * dt
    else:
        if cmd > 0:
            speed += (cmd * ACCEL_KMH_PER_S) * dt
        else:
            speed -= COAST_KMH_PER_S * dt  # sueltas o jalas abajo: desacelera

    speed = clamp(speed, 0.0, MAX_KMH)

    # ---------- OLED ----------
    oled.fill(0)
    oled.text("JOY SPD", 0, 0)
    oled.text(f"{int(speed + 0.5)} km/h", 0, 20)
    oled.text(f"cmd:{cmd:+.2f}", 0, 48)
    if braking:
        oled.text("BRK", 100, 0)
    oled.show()

    time.sleep_ms(DT_MS)

# MicroPython SSD1306 OLED driver, I2C and SPI interfaces
from micropython import const
import framebuf

# register definitions
SET_CONTRAST        = const(0x81)
SET_ENTIRE_ON       = const(0xA4)
SET_NORM_INV        = const(0xA6)
SET_DISP            = const(0xAE)
SET_MEM_ADDR        = const(0x20)
SET_COL_ADDR        = const(0x21)
SET_PAGE_ADDR       = const(0x22)
SET_DISP_START_LINE = const(0x40)
SET_SEG_REMAP       = const(0xA0)
SET_MUX_RATIO       = const(0xA8)
SET_COM_OUT_DIR     = const(0xC0)
SET_DISP_OFFSET     = const(0xD3)
SET_COM_PIN_CFG     = const(0xDA)
SET_DISP_CLK_DIV    = const(0xD5)
SET_PRECHARGE       = const(0xD9)
SET_VCOM_DESEL      = const(0xDB)
SET_CHARGE_PUMP     = const(0x8D)

class SSD1306(framebuf.FrameBuffer):
    def __init__(self, width, height, external_vcc):
        self.width = width
        self.height = height
        self.external_vcc = external_vcc
        self.pages = self.height // 8
        self.buffer = bytearray(self.pages * self.width)
        super().__init__(self.buffer, self.width, self.height, framebuf.MONO_VLSB)
        self.init_display()

    def init_display(self):
        for cmd in (
            SET_DISP | 0x00,               # off
            SET_MEM_ADDR, 0x00,            # horizontal addressing
            SET_DISP_START_LINE | 0x00,
            SET_SEG_REMAP | 0x01,          # rotate
            SET_MUX_RATIO, self.height - 1,
            SET_COM_OUT_DIR | 0x08,        # rotate
            SET_DISP_OFFSET, 0x00,
            SET_COM_PIN_CFG, 0x12 if self.height == 64 else 0x02,
            SET_DISP_CLK_DIV, 0x80,
            SET_PRECHARGE, 0x22 if self.external_vcc else 0xF1,
            SET_VCOM_DESEL, 0x30,
            SET_CONTRAST, 0xFF,
            SET_ENTIRE_ON,
            SET_NORM_INV,
            SET_CHARGE_PUMP, 0x10 if self.external_vcc else 0x14,
            SET_DISP | 0x01                # on
        ):
            self.write_cmd(cmd)
        self.fill(0)
        self.show()

    def poweroff(self):
        self.write_cmd(SET_DISP | 0x00)

    def poweron(self):
        self.write_cmd(SET_DISP | 0x01)

    def contrast(self, contrast):
        self.write_cmd(SET_CONTRAST)
        self.write_cmd(contrast)

    def invert(self, invert):
        self.write_cmd(SET_NORM_INV | (invert & 1))

    def show(self):
        self.write_cmd(SET_COL_ADDR)
        self.write_cmd(0)
        self.write_cmd(self.width - 1)
        self.write_cmd(SET_PAGE_ADDR)
        self.write_cmd(0)
        self.write_cmd(self.pages - 1)
        self.write_data(self.buffer)

    def write_cmd(self, cmd):
        raise NotImplementedError

    def write_data(self, buf):
        raise NotImplementedError

class SSD1306_I2C(SSD1306):
    def __init__(self, width, height, i2c, addr=0x3C, external_vcc=False):
        self.i2c = i2c
        self.addr = addr
        self.temp = bytearray(2)
        super().__init__(width, height, external_vcc)

    def write_cmd(self, cmd):
        self.temp[0] = 0x80
        self.temp[1] = cmd
        self.i2c.writeto(self.addr, self.temp)

    def write_data(self, buf):
        # 0x40 = data stream
        self.i2c.writeto(self.addr, b"\x40" + buf)

#include 
#include 
#include 

#include "pico/stdlib.h"
#include "hardware/adc.h"

// ---------------- Pines ----------------
#define DIST_ADC_PIN   26   // GP26 = ADC0 (sensor distancia)
#define DIST_ADC_CH    0

#define POT_ADC_PIN    27   // GP27 = ADC1 (pot 10k)
#define POT_ADC_CH     1

#define BUZZER_PIN     15   // GPIO buzzer (activo: ON/OFF)

// ---------------- ADC ----------------
static constexpr float ADC_MAX  = 4095.0f; // 12-bit
static constexpr float ADC_VREF = 3.3f;    // referencia ADC (aprox.)

// ---- Ajustes sensor distancia (asumiendo Sharp ~10–80cm) ----
static constexpr float V_AT_10CM = 2.3f;
static constexpr float V_AT_80CM = 0.4f;

static inline float volts_to_cm(float v) {
    return 29.988f * powf(v, -1.173f);
}

static inline float clampf(float x, float a, float b) {
    return (x < a) ? a : (x > b ? b : x);
}

static inline float read_adc_volts(uint adc_channel) {
    adc_select_input(adc_channel);
    uint16_t raw = adc_read(); // 0..4095
    return (raw * ADC_VREF) / ADC_MAX;
}

using std::string;

enum Mode {
    MODE_IDLE,
    MODE_DISTANCE,
    MODE_SPEED,
    MODE_ALL
};

static void print_help() {
    printf("\n--- MENU ---\n");
    printf("h           : help\n");
    printf("d           : modo distancia (cm)\n");
    printf("v           : modo velocidad (pot %%)\n");
    printf("a           : ambos\n");
    printf("stop        : pausa (no imprime)\n");
    printf("b on / b off: buzzer ON/OFF\n");
    printf("t       : umbral buzzer en cm (ej: t 10)\n");
    printf("p           : imprime estado actual\n\n");
}

int main() {
    stdio_init_all();
    sleep_ms(2000);

    printf("\n[Pico listo] Menu skate (distancia + acelerador)\n");
    print_help();

    // ADC init
    adc_init();
    adc_gpio_init(DIST_ADC_PIN);
    adc_gpio_init(POT_ADC_PIN);

    // Buzzer GPIO (activo ON/OFF)
    gpio_init(BUZZER_PIN);
    gpio_set_dir(BUZZER_PIN, GPIO_OUT);
    gpio_put(BUZZER_PIN, 0);

    Mode mode = MODE_ALL;

    bool buzzer_enabled = true;
    float threshold_cm = 10.0f;

    // Beep intermitente
    bool beep_state = false;
    absolute_time_t next_beep_toggle = get_absolute_time();

    // Print periódico
    const uint32_t period_ms = 100; // 10 Hz
    absolute_time_t next_print = make_timeout_time_ms(period_ms);

    string cmd = "";

    while (true) {
        // --------- Comandos por consola (no bloqueante) ---------
        int ch = getchar_timeout_us(0);
        if (ch != PICO_ERROR_TIMEOUT) {
            if (ch == '\n' || ch == '\r') {
                if (!cmd.empty()) {
                    if (cmd == "h" || cmd == "help") {
                        print_help();
                    } else if (cmd == "d") {
                        mode = MODE_DISTANCE;
                        printf("[OK] Modo distancia\n");
                    } else if (cmd == "v") {
                        mode = MODE_SPEED;
                        printf("[OK] Modo velocidad\n");
                    } else if (cmd == "a") {
                        mode = MODE_ALL;
                        printf("[OK] Modo ambos\n");
                    } else if (cmd == "stop") {
                        mode = MODE_IDLE;
                        printf("[OK] Pausa\n");
                    } else if (cmd == "b on") {
                        buzzer_enabled = true;
                        printf("[OK] Buzzer ON\n");
                    } else if (cmd == "b off") {
                        buzzer_enabled = false;
                        gpio_put(BUZZER_PIN, 0);
                        printf("[OK] Buzzer OFF\n");
                    } else if (cmd.size() >= 2 && cmd[0] == 't' && cmd[1] == ' ') {
                        float v = atof(cmd.c_str() + 2);
                        if (v > 0.0f && v < 200.0f) {
                            threshold_cm = v;
                            printf("[OK] Umbral = %.1f cm\n", threshold_cm);
                        } else {
                            printf("[ERR] Umbral invalido\n");
                        }
                    } else if (cmd == "p") {
                        const char* m =
                            (mode == MODE_IDLE) ? "IDLE" :
                            (mode == MODE_DISTANCE) ? "DIST" :
                            (mode == MODE_SPEED) ? "SPEED" : "ALL";
                        printf("[Estado] mode=%s  buzzer=%s  th=%.1fcm\n",
                               m, buzzer_enabled ? "ON" : "OFF", threshold_cm);
                    } else {
                        printf("[ERR] Comando desconocido: %s\n", cmd.c_str());
                    }
                    cmd.clear();
                }
            } else {
                cmd += (char)ch;
            }
        }

        // --------- Lecturas + salida periódica ---------
        if (absolute_time_diff_us(get_absolute_time(), next_print) <= 0) {
            float dist_cm = -1.0f;
            float speed_pct = -1.0f;

            // Distancia
            if (mode == MODE_DISTANCE || mode == MODE_ALL) {
                const int N = 8;
                float v_acc = 0.0f;
                for (int i = 0; i < N; i++) {
                    v_acc += read_adc_volts(DIST_ADC_CH);
                    sleep_us(200);
                }
                float v = v_acc / N;

                if (v > V_AT_10CM) dist_cm = 10.0f;
                else if (v < V_AT_80CM) dist_cm = 80.0f;
                else dist_cm = volts_to_cm(v);

                dist_cm = clampf(dist_cm, 10.0f, 80.0f);
            }

            // Potenciómetro => acelerador %
            if (mode == MODE_SPEED || mode == MODE_ALL) {
                const int N = 8;
                uint32_t acc = 0;
                adc_select_input(POT_ADC_CH);
                for (int i = 0; i < N; i++) {
                    acc += adc_read();
                    sleep_us(200);
                }
                float raw = (float)acc / (float)N; // 0..4095
                speed_pct = (raw / ADC_MAX) * 100.0f;
                speed_pct = clampf(speed_pct, 0.0f, 100.0f);
            }

            if (mode == MODE_DISTANCE) {
                printf("dist_cm=%.1f\n", dist_cm);
            } else if (mode == MODE_SPEED) {
                printf("speed=%.0f%%\n", speed_pct);
            } else if (mode == MODE_ALL) {
                printf("dist_cm=%.1f  speed=%.0f%%\n", dist_cm, speed_pct);
            }

            // --------- Buzzer por umbral ---------
            if (buzzer_enabled && (mode == MODE_DISTANCE || mode == MODE_ALL)) {
                if (dist_cm > 0.0f && dist_cm <= threshold_cm) {
                    // Toggle cada 200ms
                    if (absolute_time_diff_us(get_absolute_time(), next_beep_toggle) <= 0) {
                        beep_state = !beep_state;
                        gpio_put(BUZZER_PIN, beep_state ? 1 : 0);
                        next_beep_toggle = make_timeout_time_ms(200);
                    }
                } else {
                    beep_state = false;
                    gpio_put(BUZZER_PIN, 0);
                    next_beep_toggle = get_absolute_time();
                }
            } else {
                beep_state = false;
                gpio_put(BUZZER_PIN, 0);
            }

            next_print = delayed_by_ms(next_print, period_ms);
        }

        sleep_ms(5);
    }
}