#include <stdio.h>
#include "pico/stdlib.h"
#include "hardware/pwm.h"
#include "hardware/clocks.h"
#include "hardware/adc.h"
#include "hardware/gpio.h"
#include "hardware/i2c.h"
#include "pico/cyw43_arch.h"
#include <string.h>
#include <math.h>
#define SERVO_PIN 15
#define ENCODER_PIN 28
#define ADC_CHAN 2 // ADC channel for GPIO 28
#define SSD1306_I2C_ADDR 0x3C
#define SSD1306_HEIGHT 64
#define SSD1306_WIDTH 128
#define SSD1306_PAGE_HEIGHT 8
#define SSD1306_NUM_PAGES (SSD1306_HEIGHT / SSD1306_PAGE_HEIGHT)
#define SSD1306_BUF_LEN (SSD1306_NUM_PAGES * SSD1306_WIDTH)
#define I2C_PORT i2c0
#define I2C_SDA 4
#define I2C_SCL 5
#define I2C_FREQ 1000000
// ssd1306 command definitions
#define SSD1306_SET_CONTRAST 0x81
#define SSD1306_SET_ENTIRE_ON 0xA4
#define SSD1306_SET_NORM_INV 0xA6
#define SSD1306_SET_DISP 0xAE
#define SSD1306_SET_MEM_ADDR 0x20
#define SSD1306_SET_COL_ADDR 0x21
#define SSD1306_SET_PAGE_ADDR 0x22
#define SSD1306_SET_DISP_START_LINE 0x40
#define SSD1306_SET_SEG_REMAP 0xA0
#define SSD1306_SET_MUX_RATIO 0xA8
#define SSD1306_SET_COM_OUT_DIR 0xC0
#define SSD1306_SET_DISP_OFFSET 0xD3
#define SSD1306_SET_COM_PIN_CFG 0xDA
#define SSD1306_SET_DISP_CLK_DIV 0xD5
#define SSD1306_SET_PRECHARGE 0xD9
#define SSD1306_SET_VCOM_DESEL 0xDB
#define SSD1306_SET_CHARGE_PUMP 0x8D
typedef struct {
float x, y, z;
} Vector3D;
Vector3D cube_vertices[8] = {
{-1, -1, -1}, {1, -1, -1}, {1, 1, -1}, {-1, 1, -1},
{-1, -1, 1}, {1, -1, 1}, {1, 1, 1}, {-1, 1, 1}
};
typedef struct {
int x, y;
} Pixel;
Pixel pixel_a = {-1, -1};
Pixel pixel_b = {-1, -1};
void SSD1306_send_cmd(uint8_t cmd) {
uint8_t buf[2] = {0x80, cmd};
i2c_write_blocking(I2C_PORT, SSD1306_I2C_ADDR, buf, 2, false);
}
void SSD1306_send_data(uint8_t *buf, int buflen) {
uint8_t *temp_buf = malloc(buflen 1);
temp_buf[0] = 0x40;
memcpy(temp_buf 1, buf, buflen);
i2c_write_blocking(I2C_PORT, SSD1306_I2C_ADDR, temp_buf, buflen 1, false);
free(temp_buf);
}
void SSD1306_init() {
// Turn off display
SSD1306_send_cmd(SSD1306_SET_DISP | 0x00);
sleep_ms(100);
// Set memory addressing mode
SSD1306_send_cmd(SSD1306_SET_MEM_ADDR);
SSD1306_send_cmd(0x00); // Horizontal addressing mode
// Set column address range
SSD1306_send_cmd(SSD1306_SET_COL_ADDR);
SSD1306_send_cmd(0);
SSD1306_send_cmd(SSD1306_WIDTH - 1);
// Set page address range
SSD1306_send_cmd(SSD1306_SET_PAGE_ADDR);
SSD1306_send_cmd(0);
SSD1306_send_cmd(SSD1306_NUM_PAGES - 1);
// Set display start line
SSD1306_send_cmd(SSD1306_SET_DISP_START_LINE | 0x00);
// Set segment re-map
SSD1306_send_cmd(SSD1306_SET_SEG_REMAP | 0x01); // Column address 127 is mapped to SEG0
// Set multiplex ratio
SSD1306_send_cmd(SSD1306_SET_MUX_RATIO);
SSD1306_send_cmd(SSD1306_HEIGHT - 1);
// Set display offset
SSD1306_send_cmd(SSD1306_SET_DISP_OFFSET);
SSD1306_send_cmd(0x00); // No offset
// Set COM pins hardware configuration
SSD1306_send_cmd(SSD1306_SET_COM_PIN_CFG);
SSD1306_send_cmd(0x12); // Alternative COM pin configuration, disable COM left/right remap
// Set display clock divide ratio and oscillator frequency
SSD1306_send_cmd(SSD1306_SET_DISP_CLK_DIV);
SSD1306_send_cmd(0x80); // Default value
// Set pre-charge period
SSD1306_send_cmd(SSD1306_SET_PRECHARGE);
SSD1306_send_cmd(0xF1); // Phase 1 = 15 DCLK, Phase 2 = 1 DCLK
// Set VCOMH deselect level
SSD1306_send_cmd(SSD1306_SET_VCOM_DESEL);
SSD1306_send_cmd(0x30); // 0.83 x VCC
// Set contrast control
SSD1306_send_cmd(SSD1306_SET_CONTRAST);
SSD1306_send_cmd(0xFF); // Maximum
// Disable entire display on
SSD1306_send_cmd(SSD1306_SET_ENTIRE_ON);
// Set normal display (not inverted)
SSD1306_send_cmd(SSD1306_SET_NORM_INV);
// Set charge pump
SSD1306_send_cmd(SSD1306_SET_CHARGE_PUMP);
SSD1306_send_cmd(0x14); // Enable charge pump
// Deactivate scroll
SSD1306_send_cmd(0x2E);
// Clear the display buffer
uint8_t clear_buf[SSD1306_BUF_LEN] = {0};
SSD1306_send_data(clear_buf, sizeof(clear_buf));
sleep_ms(100);
// Turn on display
SSD1306_send_cmd(SSD1306_SET_DISP | 0x01);
sleep_ms(100);
}
void set_pixel(uint8_t *buffer, int x, int y) {
if (x < 0 || x >= SSD1306_WIDTH || y < 0 || y >= SSD1306_HEIGHT) return;
int byte_idx = (y / 8) * SSD1306_WIDTH x;
buffer[byte_idx] |= 1 << (y % 8);
}
void set_line(uint8_t *buffer, int x0, int y0, int x1, int y1) {
int dx = abs(x1 - x0), sx = x0 < x1 ? 1 : -1;
int dy = abs(y1 - y0), sy = y0 < y1 ? 1 : -1;
int err = (dx > dy ? dx : -dy) / 2;
while (true) {
set_pixel(buffer, x0, y0);
if (x0 == x1 && y0 == y1) break;
int e2 = err;
if (e2 > -dx) { err -= dy; x0 = sx; }
if (e2 < dy) { err = dx; y0 = sy; }
}
}
void rotate_point(Vector3D *p, float angle_x, float angle_y, float angle_z) {
float sin_x = sin(angle_x), cos_x = cos(angle_x);
float sin_y = sin(angle_y), cos_y = cos(angle_y);
float sin_z = sin(angle_z), cos_z = cos(angle_z);
float y = p->y;
p->y = y * cos_x - p->z * sin_x;
p->z = y * sin_x p->z * cos_x;
float x = p->x;
p->x = x * cos_y p->z * sin_y;
p->z = -x * sin_y p->z * cos_y;
x = p->x;
p->x = x * cos_z - p->y * sin_z;
p->y = x * sin_z p->y * cos_z;
}
void project_point(Vector3D *p3d, int *x2d, int *y2d) {
*x2d = (int)((p3d->x * 20) / (p3d->z 3) SSD1306_WIDTH/2);
*y2d = (int)((p3d->y * 20) / (p3d->z 3) SSD1306_HEIGHT/2);
}
void draw_cube(uint8_t *buffer, float angle_x, float angle_y, float angle_z) {
Vector3D rotated_vertices[8];
int x2d[8], y2d[8];
for (int i = 0; i < 8; i ) {
rotated_vertices[i] = cube_vertices[i];
rotate_point(&rotated_vertices[i], angle_x, angle_y, angle_z);
project_point(&rotated_vertices[i], &x2d[i], &y2d[i]);
}
int edges[12][2] = {{0,1},{1,2},{2,3},{3,0},{4,5},{5,6},{6,7},{7,4},{0,4},{1,5},{2,6},{3,7}};
for (int i = 0; i < 12; i ) {
set_line(buffer, x2d[edges[i][0]], y2d[edges[i][0]], x2d[edges[i][1]], y2d[edges[i][1]]);
}
}
void draw_pixel_square(uint8_t *buffer, int x, int y) {
for (int dx = -2; dx <= 2; dx ) {
for (int dy = -2; dy <= 2; dy ) {
set_pixel(buffer, x dx, y dy);
}
}
}
void draw_custom_pixels(uint8_t *buffer) {
if (pixel_a.x >= 0 && pixel_a.y >= 0) {
draw_pixel_square(buffer, pixel_a.x, pixel_a.y);
}
if (pixel_b.x >= 0 && pixel_b.y >= 0) {
draw_pixel_square(buffer, pixel_b.x, pixel_b.y);
}
}
void parse_command(char *cmd) {
char type;
int x, y;
if (sscanf(cmd, "%c %d %d", &type, &x, &y) == 3) {
if (type == 'a') {
pixel_a.x = x;
pixel_a.y = y;
} else if (type == 'b') {
pixel_b.x = x;
pixel_b.y = y;
}
} else if (strcmp(cmd, "clear") == 0) {
pixel_a.x = pixel_a.y = pixel_b.x = pixel_b.y = -1;
}
}
float clockDiv = 64;
float wrap = 39062;
void setMicros(int servoPin, float micros) {
pwm_set_gpio_level(servoPin, (micros/20000.f)*wrap);
}
void setServo(int servoPin, float startMicros) {
gpio_set_function(servoPin, GPIO_FUNC_PWM);
uint slice_num = pwm_gpio_to_slice_num(servoPin);
pwm_config config = pwm_get_default_config();
uint64_t clockspeed = clock_get_hz(clk_sys);
while (clockspeed/clockDiv/50 > 65535 && clockDiv < 256) clockDiv = 64;
wrap = clockspeed/clockDiv/50;
pwm_config_set_clkdiv(&config, clockDiv);
pwm_config_set_wrap(&config, wrap);
pwm_init(slice_num, &config, true);
setMicros(servoPin, startMicros);
}
float read_encoder() {
adc_select_input(ADC_CHAN);
uint16_t result = adc_read();
return (float)result; // Return raw ADC value
}
int main() {
stdio_init_all();
if (cyw43_arch_init()) {
printf("wi-fi init failed");
return -1;
}
i2c_init(I2C_PORT, I2C_FREQ);
gpio_set_function(I2C_SDA, GPIO_FUNC_I2C);
gpio_set_function(I2C_SCL, GPIO_FUNC_I2C);
gpio_pull_up(I2C_SDA);
gpio_pull_up(I2C_SCL);
// Initialize ADC
adc_init();
adc_gpio_init(ENCODER_PIN);
// Initialize servo
setServo(SERVO_PIN, 1500); // start at middle position
SSD1306_init();
sleep_ms(1000);
uint8_t display_buffer[SSD1306_BUF_LEN];
float angle_x = 0, angle_y = 0, angle_z = 0;
char cmd[100];
int cmd_index = 0;
while (true) {
// Read encoder and update angles
float encoder_value = read_encoder();
angle_x = (encoder_value / 4095.0f) * 2 * M_PI;
angle_y = (encoder_value / 4095.0f) * 2 * M_PI;
angle_z = (encoder_value / 4095.0f) * 2 * M_PI;
// Update servo position based on encoder value
float position = encoder_value * (2000.0f / 4095.0f) 500.0f; // map 0-4095 to 500-2500 microseconds
setMicros(SERVO_PIN, position);
memset(display_buffer, 0, SSD1306_BUF_LEN);
draw_cube(display_buffer, angle_x, angle_y, angle_z);
draw_custom_pixels(display_buffer);
SSD1306_send_data(display_buffer, sizeof(display_buffer));
// Rest of the main loop (command parsing) remains the same
while (stdio_usb_connected()) {
int c = getchar_timeout_us(0);
if (c == PICO_ERROR_TIMEOUT) break;
if (c == '\n' || c == '\r') {
cmd[cmd_index] = '\0';
parse_command(cmd);
cmd_index = 0;
} else if (cmd_index < 99) {
cmd[cmd_index ] = c;
}
}
}
return 0;
}