Converted Code from Codecell to RPI Pico
This commit is contained in:
parent
e0bb4b3ae8
commit
48e22faf62
|
|
@ -0,0 +1 @@
|
||||||
|
build/
|
||||||
|
|
@ -12,13 +12,9 @@ pico_sdk_init()
|
||||||
|
|
||||||
add_executable(motion_controller
|
add_executable(motion_controller
|
||||||
src/main.c
|
src/main.c
|
||||||
src/robot.c
|
|
||||||
src/motors.c
|
|
||||||
src/gyro.c
|
src/gyro.c
|
||||||
src/motion_control.c
|
src/ssd1306.c
|
||||||
src/i2c_master.c
|
src/io.c
|
||||||
src/udp_client.c
|
|
||||||
src/udp_buffer.c
|
|
||||||
)
|
)
|
||||||
|
|
||||||
target_link_libraries(motion_controller
|
target_link_libraries(motion_controller
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,137 @@
|
||||||
|
#include "include/gyro.h"
|
||||||
|
|
||||||
|
#include <stdint.h>
|
||||||
|
#include <hardware/i2c.h>
|
||||||
|
#include "include/io.h"
|
||||||
|
|
||||||
|
//#include <stdio.h>
|
||||||
|
|
||||||
|
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *\
|
||||||
|
* Thank you Keuronde ! *
|
||||||
|
* https://git.poivron-robotique.fr/Keuronde/Holonome_2024/src/branch/Demo_2025_03/gyro_L3GD20H.c *
|
||||||
|
\* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
|
||||||
|
|
||||||
|
#define SAMPLE_MIN_ELAPSED_TIME 2 // ms
|
||||||
|
#define DPS_PER_DIGIT 0.00875f
|
||||||
|
|
||||||
|
int init_gyro(void)
|
||||||
|
{
|
||||||
|
// Verify gyro initialisation
|
||||||
|
uint8_t reg = 0x0f;
|
||||||
|
uint8_t data;
|
||||||
|
|
||||||
|
i2c_write_blocking(I2C_MASTER_INSTANCE, I2C_GYRO_ADDRESS, ®, 1, false);
|
||||||
|
i2c_read_blocking(I2C_MASTER_INSTANCE, I2C_GYRO_ADDRESS, &data, 1, false);
|
||||||
|
|
||||||
|
if(data != 0xd7) return -1;
|
||||||
|
|
||||||
|
// Configure gyro
|
||||||
|
const uint8_t CTRL1_REG = 0x20;
|
||||||
|
const uint8_t CTRL1_CONFIG = 0b11101111; // DR : 11 // BW : 10 // PD : 1 // Zen : 1 // Xen : 1 // Yen : 1 //
|
||||||
|
|
||||||
|
uint8_t config[] = {CTRL1_REG, CTRL1_CONFIG};
|
||||||
|
uint8_t config_verification;
|
||||||
|
|
||||||
|
i2c_write_blocking(I2C_MASTER_INSTANCE, I2C_GYRO_ADDRESS, config, 2, false);
|
||||||
|
|
||||||
|
i2c_write_blocking(I2C_MASTER_INSTANCE, I2C_GYRO_ADDRESS, &CTRL1_CONFIG, 1, false);
|
||||||
|
i2c_read_blocking(I2C_MASTER_INSTANCE, I2C_GYRO_ADDRESS, &config_verification, 1, false);
|
||||||
|
|
||||||
|
if(config_verification != config[1]) return -1;
|
||||||
|
|
||||||
|
io.gyro_data.x_angle = 0.0f;
|
||||||
|
io.gyro_data.y_angle = 0.0f;
|
||||||
|
io.gyro_data.z_angle = 0.0f;
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
static inline void __attribute__((always_inline)) gyro_read(int16_t *x, int16_t *y, int16_t *z)
|
||||||
|
{
|
||||||
|
const uint8_t X_OUT_L_REG = 0x28;
|
||||||
|
|
||||||
|
uint8_t reg = X_OUT_L_REG | 0x80; // 0x80 for auto incrementing
|
||||||
|
uint8_t data[6];
|
||||||
|
|
||||||
|
i2c_write_blocking(I2C_MASTER_INSTANCE, I2C_GYRO_ADDRESS, ®, 1, false);
|
||||||
|
i2c_read_blocking(I2C_MASTER_INSTANCE, I2C_GYRO_ADDRESS, data, 1, false);
|
||||||
|
|
||||||
|
*x = (int16_t)((data[1] << 8) | data[0]);
|
||||||
|
*y = (int16_t)((data[3] << 8) | data[2]);
|
||||||
|
*z = (int16_t)((data[5] << 8) | data[4]);
|
||||||
|
}
|
||||||
|
|
||||||
|
void gyro_calibrate(void)
|
||||||
|
{
|
||||||
|
const uint nb_samples = 10000;
|
||||||
|
|
||||||
|
int16_t x, y, z;
|
||||||
|
int32_t x_sum = 0, y_sum = 0, z_sum = 0;
|
||||||
|
|
||||||
|
for(uint i = 0; i < nb_samples; i++)
|
||||||
|
{
|
||||||
|
gyro_read(&x, &y, &z);
|
||||||
|
|
||||||
|
x_sum += x;
|
||||||
|
y_sum += y;
|
||||||
|
z_sum += z;
|
||||||
|
|
||||||
|
sleep_ms(SAMPLE_MIN_ELAPSED_TIME);
|
||||||
|
|
||||||
|
//printf(">cal_x:%d\n", x);
|
||||||
|
//printf(">cal_y:%d\n", y);
|
||||||
|
//printf(">cal_z:%d\n", z);
|
||||||
|
}
|
||||||
|
|
||||||
|
io.gyro_data.x_offset = (float)x_sum / (float)nb_samples * DPS_PER_DIGIT;
|
||||||
|
io.gyro_data.y_offset = (float)y_sum / (float)nb_samples * DPS_PER_DIGIT;
|
||||||
|
io.gyro_data.z_offset = (float)z_sum / (float)nb_samples * DPS_PER_DIGIT;
|
||||||
|
|
||||||
|
//printf("\nx_cal:%.5f\n", io.gyro_data.x_offset);
|
||||||
|
//printf("\ny_cal:%.5f\n", io.gyro_data.y_offset);
|
||||||
|
//printf("\nz_cal:%.5f\n", io.gyro_data.z_offset);
|
||||||
|
}
|
||||||
|
|
||||||
|
static inline void __attribute__((always_inline)) gyro_get_dps(double* x_dps, double* y_dps, double* z_dps)
|
||||||
|
{
|
||||||
|
int16_t x, y, z;
|
||||||
|
gyro_read(&x, &y, &z);
|
||||||
|
|
||||||
|
*x_dps = x * DPS_PER_DIGIT;
|
||||||
|
*y_dps = y * DPS_PER_DIGIT;
|
||||||
|
*z_dps = z * DPS_PER_DIGIT;
|
||||||
|
}
|
||||||
|
|
||||||
|
void gyro_update(void)
|
||||||
|
{
|
||||||
|
static double elapsed_since_sample_ms = 10.0;
|
||||||
|
|
||||||
|
elapsed_since_sample_ms += io.delta_time_ms;
|
||||||
|
|
||||||
|
if(elapsed_since_sample_ms >= SAMPLE_MIN_ELAPSED_TIME)
|
||||||
|
{
|
||||||
|
double x_dps, y_dps, z_dps;
|
||||||
|
gyro_get_dps(&x_dps, &y_dps, &z_dps);
|
||||||
|
|
||||||
|
x_dps -= io.gyro_data.x_offset;
|
||||||
|
y_dps -= io.gyro_data.y_offset;
|
||||||
|
z_dps -= io.gyro_data.z_offset;
|
||||||
|
|
||||||
|
io.gyro_data.x_angle += x_dps * elapsed_since_sample_ms / 1000.0f;
|
||||||
|
io.gyro_data.y_angle += y_dps * elapsed_since_sample_ms / 1000.0f;
|
||||||
|
io.gyro_data.z_angle += z_dps * elapsed_since_sample_ms / 1000.0f;
|
||||||
|
|
||||||
|
while(io.gyro_data.x_angle > 180) io.gyro_data.x_angle -= 360;
|
||||||
|
while(io.gyro_data.x_angle < -180) io.gyro_data.x_angle += 360;
|
||||||
|
while(io.gyro_data.y_angle > 180) io.gyro_data.y_angle -= 360;
|
||||||
|
while(io.gyro_data.y_angle < -180) io.gyro_data.y_angle += 360;
|
||||||
|
while(io.gyro_data.z_angle > 180) io.gyro_data.z_angle -= 360;
|
||||||
|
while(io.gyro_data.z_angle < -180) io.gyro_data.z_angle += 360;
|
||||||
|
|
||||||
|
//printf(">gyro_x_angle:%f\n", io.gyro_data.x_angle);
|
||||||
|
//printf(">gyro_y_angle:%f\n", io.gyro_data.y_angle);
|
||||||
|
//printf(">gyro_z_angle:%f\n", io.gyro_data.z_angle);
|
||||||
|
|
||||||
|
elapsed_since_sample_ms = 0.0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,110 @@
|
||||||
|
#ifndef _inc_font
|
||||||
|
#define _inc_font
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Format
|
||||||
|
* <height>, <width>, <additional spacing per char>,
|
||||||
|
* <first ascii char>, <last ascii char>,
|
||||||
|
* <data>
|
||||||
|
*/
|
||||||
|
const uint8_t font_8x5[] =
|
||||||
|
{
|
||||||
|
8, 5, 1, 32, 126,
|
||||||
|
0x00, 0x00, 0x00, 0x00, 0x00,
|
||||||
|
0x00, 0x00, 0x5F, 0x00, 0x00,
|
||||||
|
0x00, 0x07, 0x00, 0x07, 0x00,
|
||||||
|
0x14, 0x7F, 0x14, 0x7F, 0x14,
|
||||||
|
0x24, 0x2A, 0x7F, 0x2A, 0x12,
|
||||||
|
0x23, 0x13, 0x08, 0x64, 0x62,
|
||||||
|
0x36, 0x49, 0x56, 0x20, 0x50,
|
||||||
|
0x00, 0x08, 0x07, 0x03, 0x00,
|
||||||
|
0x00, 0x1C, 0x22, 0x41, 0x00,
|
||||||
|
0x00, 0x41, 0x22, 0x1C, 0x00,
|
||||||
|
0x2A, 0x1C, 0x7F, 0x1C, 0x2A,
|
||||||
|
0x08, 0x08, 0x3E, 0x08, 0x08,
|
||||||
|
0x00, 0x80, 0x70, 0x30, 0x00,
|
||||||
|
0x08, 0x08, 0x08, 0x08, 0x08,
|
||||||
|
0x00, 0x00, 0x60, 0x60, 0x00,
|
||||||
|
0x20, 0x10, 0x08, 0x04, 0x02,
|
||||||
|
0x3E, 0x51, 0x49, 0x45, 0x3E,
|
||||||
|
0x00, 0x42, 0x7F, 0x40, 0x00,
|
||||||
|
0x72, 0x49, 0x49, 0x49, 0x46,
|
||||||
|
0x21, 0x41, 0x49, 0x4D, 0x33,
|
||||||
|
0x18, 0x14, 0x12, 0x7F, 0x10,
|
||||||
|
0x27, 0x45, 0x45, 0x45, 0x39,
|
||||||
|
0x3C, 0x4A, 0x49, 0x49, 0x31,
|
||||||
|
0x41, 0x21, 0x11, 0x09, 0x07,
|
||||||
|
0x36, 0x49, 0x49, 0x49, 0x36,
|
||||||
|
0x46, 0x49, 0x49, 0x29, 0x1E,
|
||||||
|
0x00, 0x00, 0x14, 0x00, 0x00,
|
||||||
|
0x00, 0x40, 0x34, 0x00, 0x00,
|
||||||
|
0x00, 0x08, 0x14, 0x22, 0x41,
|
||||||
|
0x14, 0x14, 0x14, 0x14, 0x14,
|
||||||
|
0x00, 0x41, 0x22, 0x14, 0x08,
|
||||||
|
0x02, 0x01, 0x59, 0x09, 0x06,
|
||||||
|
0x3E, 0x41, 0x5D, 0x59, 0x4E,
|
||||||
|
0x7C, 0x12, 0x11, 0x12, 0x7C,
|
||||||
|
0x7F, 0x49, 0x49, 0x49, 0x36,
|
||||||
|
0x3E, 0x41, 0x41, 0x41, 0x22,
|
||||||
|
0x7F, 0x41, 0x41, 0x41, 0x3E,
|
||||||
|
0x7F, 0x49, 0x49, 0x49, 0x41,
|
||||||
|
0x7F, 0x09, 0x09, 0x09, 0x01,
|
||||||
|
0x3E, 0x41, 0x41, 0x51, 0x73,
|
||||||
|
0x7F, 0x08, 0x08, 0x08, 0x7F,
|
||||||
|
0x00, 0x41, 0x7F, 0x41, 0x00,
|
||||||
|
0x20, 0x40, 0x41, 0x3F, 0x01,
|
||||||
|
0x7F, 0x08, 0x14, 0x22, 0x41,
|
||||||
|
0x7F, 0x40, 0x40, 0x40, 0x40,
|
||||||
|
0x7F, 0x02, 0x1C, 0x02, 0x7F,
|
||||||
|
0x7F, 0x04, 0x08, 0x10, 0x7F,
|
||||||
|
0x3E, 0x41, 0x41, 0x41, 0x3E,
|
||||||
|
0x7F, 0x09, 0x09, 0x09, 0x06,
|
||||||
|
0x3E, 0x41, 0x51, 0x21, 0x5E,
|
||||||
|
0x7F, 0x09, 0x19, 0x29, 0x46,
|
||||||
|
0x26, 0x49, 0x49, 0x49, 0x32,
|
||||||
|
0x03, 0x01, 0x7F, 0x01, 0x03,
|
||||||
|
0x3F, 0x40, 0x40, 0x40, 0x3F,
|
||||||
|
0x1F, 0x20, 0x40, 0x20, 0x1F,
|
||||||
|
0x3F, 0x40, 0x38, 0x40, 0x3F,
|
||||||
|
0x63, 0x14, 0x08, 0x14, 0x63,
|
||||||
|
0x03, 0x04, 0x78, 0x04, 0x03,
|
||||||
|
0x61, 0x59, 0x49, 0x4D, 0x43,
|
||||||
|
0x00, 0x7F, 0x41, 0x41, 0x41,
|
||||||
|
0x02, 0x04, 0x08, 0x10, 0x20,
|
||||||
|
0x00, 0x41, 0x41, 0x41, 0x7F,
|
||||||
|
0x04, 0x02, 0x01, 0x02, 0x04,
|
||||||
|
0x40, 0x40, 0x40, 0x40, 0x40,
|
||||||
|
0x00, 0x03, 0x07, 0x08, 0x00,
|
||||||
|
0x20, 0x54, 0x54, 0x78, 0x40,
|
||||||
|
0x7F, 0x28, 0x44, 0x44, 0x38,
|
||||||
|
0x38, 0x44, 0x44, 0x44, 0x28,
|
||||||
|
0x38, 0x44, 0x44, 0x28, 0x7F,
|
||||||
|
0x38, 0x54, 0x54, 0x54, 0x18,
|
||||||
|
0x00, 0x08, 0x7E, 0x09, 0x02,
|
||||||
|
0x18, 0xA4, 0xA4, 0x9C, 0x78,
|
||||||
|
0x7F, 0x08, 0x04, 0x04, 0x78,
|
||||||
|
0x00, 0x44, 0x7D, 0x40, 0x00,
|
||||||
|
0x20, 0x40, 0x40, 0x3D, 0x00,
|
||||||
|
0x7F, 0x10, 0x28, 0x44, 0x00,
|
||||||
|
0x00, 0x41, 0x7F, 0x40, 0x00,
|
||||||
|
0x7C, 0x04, 0x78, 0x04, 0x78,
|
||||||
|
0x7C, 0x08, 0x04, 0x04, 0x78,
|
||||||
|
0x38, 0x44, 0x44, 0x44, 0x38,
|
||||||
|
0xFC, 0x18, 0x24, 0x24, 0x18,
|
||||||
|
0x18, 0x24, 0x24, 0x18, 0xFC,
|
||||||
|
0x7C, 0x08, 0x04, 0x04, 0x08,
|
||||||
|
0x48, 0x54, 0x54, 0x54, 0x24,
|
||||||
|
0x04, 0x04, 0x3F, 0x44, 0x24,
|
||||||
|
0x3C, 0x40, 0x40, 0x20, 0x7C,
|
||||||
|
0x1C, 0x20, 0x40, 0x20, 0x1C,
|
||||||
|
0x3C, 0x40, 0x30, 0x40, 0x3C,
|
||||||
|
0x44, 0x28, 0x10, 0x28, 0x44,
|
||||||
|
0x4C, 0x90, 0x90, 0x90, 0x7C,
|
||||||
|
0x44, 0x64, 0x54, 0x4C, 0x44,
|
||||||
|
0x00, 0x08, 0x36, 0x41, 0x00,
|
||||||
|
0x00, 0x00, 0x77, 0x00, 0x00,
|
||||||
|
0x00, 0x41, 0x36, 0x08, 0x00,
|
||||||
|
0x02, 0x01, 0x02, 0x04, 0x02,
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,18 @@
|
||||||
|
#ifndef GYRO_H
|
||||||
|
#define GYRO_H
|
||||||
|
|
||||||
|
#define I2C_GYRO_ADDRESS 0x6b
|
||||||
|
|
||||||
|
typedef struct gyro_data_t {
|
||||||
|
float x_offset, y_offset, z_offset;
|
||||||
|
float x_angle, y_angle, z_angle;
|
||||||
|
} gyro_data_t;
|
||||||
|
|
||||||
|
// Check if gyro has correctly initialised and configure it for simple use
|
||||||
|
int init_gyro(void);
|
||||||
|
// Calibrate gyro
|
||||||
|
void gyro_calibrate(void);
|
||||||
|
// Update gyro data
|
||||||
|
void gyro_update(void);
|
||||||
|
|
||||||
|
#endif // GYRO_H
|
||||||
|
|
@ -0,0 +1,53 @@
|
||||||
|
#ifndef IO_H
|
||||||
|
#define IO_H
|
||||||
|
|
||||||
|
#include <pico/types.h>
|
||||||
|
#include "ssd1306.h"
|
||||||
|
#include "gyro.h"
|
||||||
|
|
||||||
|
#define I2C_MASTER_SDA_PIN 21
|
||||||
|
#define I2C_MASTER_SCL_PIN 20
|
||||||
|
#define I2C_MASTER_INSTANCE i2c0
|
||||||
|
|
||||||
|
#define PIN_TIRETTE 5
|
||||||
|
#define PIN_BUTTON_COLOR 6
|
||||||
|
#define PIN_MOTOR1 1
|
||||||
|
#define PIN_MOTOR2 2
|
||||||
|
#define PIN_SERVO 7
|
||||||
|
|
||||||
|
#define DANCING_ACTION_DELTA_ANGLE 2
|
||||||
|
|
||||||
|
#define GAIN_KD 50
|
||||||
|
#define ANGULAR_SPEED 120
|
||||||
|
|
||||||
|
typedef struct io_t {
|
||||||
|
double delta_time_ms;
|
||||||
|
|
||||||
|
bool motion_control_activated;
|
||||||
|
bool is_tirette_pulled;
|
||||||
|
bool is_color_blue;
|
||||||
|
bool is_dancing;
|
||||||
|
|
||||||
|
ssd1306_t *screen;
|
||||||
|
|
||||||
|
gyro_data_t gyro_data;
|
||||||
|
|
||||||
|
float target_dir;
|
||||||
|
float target_speed;
|
||||||
|
|
||||||
|
double time_ms;
|
||||||
|
} io_t;
|
||||||
|
|
||||||
|
extern io_t io;
|
||||||
|
|
||||||
|
void motor_control_activated(bool activated);
|
||||||
|
|
||||||
|
void init_io(void);
|
||||||
|
|
||||||
|
void update_io(void);
|
||||||
|
|
||||||
|
void set_dir_with_angular_speed(float dir);
|
||||||
|
|
||||||
|
void io_deinit(void);
|
||||||
|
|
||||||
|
#endif // IO_H
|
||||||
|
|
@ -0,0 +1,273 @@
|
||||||
|
/*
|
||||||
|
MIT License
|
||||||
|
|
||||||
|
Copyright (c) 2021 David Schramm
|
||||||
|
|
||||||
|
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||||
|
of this software and associated documentation files (the "Software"), to deal
|
||||||
|
in the Software without restriction, including without limitation the rights
|
||||||
|
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||||
|
copies of the Software, and to permit persons to whom the Software is
|
||||||
|
furnished to do so, subject to the following conditions:
|
||||||
|
|
||||||
|
The above copyright notice and this permission notice shall be included in all
|
||||||
|
copies or substantial portions of the Software.
|
||||||
|
|
||||||
|
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||||
|
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||||
|
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||||
|
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||||
|
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||||
|
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||||
|
SOFTWARE.
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file ssd1306.h
|
||||||
|
*
|
||||||
|
* simple driver for ssd1306 displays
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef _inc_ssd1306
|
||||||
|
#define _inc_ssd1306
|
||||||
|
#include <hardware/i2c.h>
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief defines commands used in ssd1306
|
||||||
|
*/
|
||||||
|
typedef enum {
|
||||||
|
SET_CONTRAST = 0x81,
|
||||||
|
SET_ENTIRE_ON = 0xA4,
|
||||||
|
SET_NORM_INV = 0xA6,
|
||||||
|
SET_DISP = 0xAE,
|
||||||
|
SET_MEM_ADDR = 0x20,
|
||||||
|
SET_COL_ADDR = 0x21,
|
||||||
|
SET_PAGE_ADDR = 0x22,
|
||||||
|
SET_DISP_START_LINE = 0x40,
|
||||||
|
SET_SEG_REMAP = 0xA0,
|
||||||
|
SET_MUX_RATIO = 0xA8,
|
||||||
|
SET_COM_OUT_DIR = 0xC0,
|
||||||
|
SET_DISP_OFFSET = 0xD3,
|
||||||
|
SET_COM_PIN_CFG = 0xDA,
|
||||||
|
SET_DISP_CLK_DIV = 0xD5,
|
||||||
|
SET_PRECHARGE = 0xD9,
|
||||||
|
SET_VCOM_DESEL = 0xDB,
|
||||||
|
SET_CHARGE_PUMP = 0x8D
|
||||||
|
} ssd1306_command_t;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief holds the configuration
|
||||||
|
*/
|
||||||
|
typedef struct {
|
||||||
|
uint8_t width; /**< width of display */
|
||||||
|
uint8_t height; /**< height of display */
|
||||||
|
uint8_t pages; /**< stores pages of display (calculated on initialization*/
|
||||||
|
uint8_t address; /**< i2c address of display*/
|
||||||
|
i2c_inst_t *i2c_i; /**< i2c connection instance */
|
||||||
|
bool external_vcc; /**< whether display uses external vcc */
|
||||||
|
uint8_t *buffer; /**< display buffer */
|
||||||
|
size_t bufsize; /**< buffer size */
|
||||||
|
} ssd1306_t;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief initialize display
|
||||||
|
*
|
||||||
|
* @param[in] p : pointer to instance of ssd1306_t
|
||||||
|
* @param[in] width : width of display
|
||||||
|
* @param[in] height : heigth of display
|
||||||
|
* @param[in] address : i2c address of display
|
||||||
|
* @param[in] i2c_instance : instance of i2c connection
|
||||||
|
*
|
||||||
|
* @return bool.
|
||||||
|
* @retval true for Success
|
||||||
|
* @retval false if initialization failed
|
||||||
|
*/
|
||||||
|
bool ssd1306_init(ssd1306_t *p, uint16_t width, uint16_t height, uint8_t address, i2c_inst_t *i2c_instance);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief deinitialize display
|
||||||
|
*
|
||||||
|
* @param[in] p : instance of display
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
void ssd1306_deinit(ssd1306_t *p);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief turn off display
|
||||||
|
*
|
||||||
|
* @param[in] p : instance of display
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
void ssd1306_poweroff(ssd1306_t *p);
|
||||||
|
|
||||||
|
/**
|
||||||
|
@brief turn on display
|
||||||
|
|
||||||
|
@param[in] p : instance of display
|
||||||
|
|
||||||
|
*/
|
||||||
|
void ssd1306_poweron(ssd1306_t *p);
|
||||||
|
|
||||||
|
/**
|
||||||
|
@brief set contrast of display
|
||||||
|
|
||||||
|
@param[in] p : instance of display
|
||||||
|
@param[in] val : contrast
|
||||||
|
|
||||||
|
*/
|
||||||
|
void ssd1306_contrast(ssd1306_t *p, uint8_t val);
|
||||||
|
|
||||||
|
/**
|
||||||
|
@brief set invert display
|
||||||
|
|
||||||
|
@param[in] p : instance of display
|
||||||
|
@param[in] inv : inv==0: disable inverting, inv!=0: invert
|
||||||
|
|
||||||
|
*/
|
||||||
|
void ssd1306_invert(ssd1306_t *p, uint8_t inv);
|
||||||
|
|
||||||
|
/**
|
||||||
|
@brief display buffer, should be called on change
|
||||||
|
|
||||||
|
@param[in] p : instance of display
|
||||||
|
|
||||||
|
*/
|
||||||
|
void ssd1306_show(ssd1306_t *p);
|
||||||
|
|
||||||
|
/**
|
||||||
|
@brief clear display buffer
|
||||||
|
|
||||||
|
@param[in] p : instance of display
|
||||||
|
|
||||||
|
*/
|
||||||
|
void ssd1306_clear(ssd1306_t *p);
|
||||||
|
|
||||||
|
/**
|
||||||
|
@brief clear pixel on buffer
|
||||||
|
|
||||||
|
@param[in] p : instance of display
|
||||||
|
@param[in] x : x position
|
||||||
|
@param[in] y : y position
|
||||||
|
*/
|
||||||
|
void ssd1306_clear_pixel(ssd1306_t *p, uint32_t x, uint32_t y);
|
||||||
|
|
||||||
|
/**
|
||||||
|
@brief draw pixel on buffer
|
||||||
|
|
||||||
|
@param[in] p : instance of display
|
||||||
|
@param[in] x : x position
|
||||||
|
@param[in] y : y position
|
||||||
|
*/
|
||||||
|
void ssd1306_draw_pixel(ssd1306_t *p, uint32_t x, uint32_t y);
|
||||||
|
|
||||||
|
/**
|
||||||
|
@brief draw line on buffer
|
||||||
|
|
||||||
|
@param[in] p : instance of display
|
||||||
|
@param[in] x1 : x position of starting point
|
||||||
|
@param[in] y1 : y position of starting point
|
||||||
|
@param[in] x2 : x position of end point
|
||||||
|
@param[in] y2 : y position of end point
|
||||||
|
*/
|
||||||
|
void ssd1306_draw_line(ssd1306_t *p, int32_t x1, int32_t y1, int32_t x2, int32_t y2);
|
||||||
|
|
||||||
|
/**
|
||||||
|
@brief clear square at given position with given size
|
||||||
|
|
||||||
|
@param[in] p : instance of display
|
||||||
|
@param[in] x : x position of starting point
|
||||||
|
@param[in] y : y position of starting point
|
||||||
|
@param[in] width : width of square
|
||||||
|
@param[in] height : height of square
|
||||||
|
*/
|
||||||
|
void ssd1306_clear_square(ssd1306_t *p, uint32_t x, uint32_t y, uint32_t width, uint32_t height);
|
||||||
|
|
||||||
|
/**
|
||||||
|
@brief draw filled square at given position with given size
|
||||||
|
|
||||||
|
@param[in] p : instance of display
|
||||||
|
@param[in] x : x position of starting point
|
||||||
|
@param[in] y : y position of starting point
|
||||||
|
@param[in] width : width of square
|
||||||
|
@param[in] height : height of square
|
||||||
|
*/
|
||||||
|
void ssd1306_draw_square(ssd1306_t *p, uint32_t x, uint32_t y, uint32_t width, uint32_t height);
|
||||||
|
|
||||||
|
/**
|
||||||
|
@brief draw empty square at given position with given size
|
||||||
|
|
||||||
|
@param[in] p : instance of display
|
||||||
|
@param[in] x : x position of starting point
|
||||||
|
@param[in] y : y position of starting point
|
||||||
|
@param[in] width : width of square
|
||||||
|
@param[in] height : height of square
|
||||||
|
*/
|
||||||
|
void ssd1306_draw_empty_square(ssd1306_t *p, uint32_t x, uint32_t y, uint32_t width, uint32_t height);
|
||||||
|
|
||||||
|
/**
|
||||||
|
@brief draw monochrome bitmap with offset
|
||||||
|
|
||||||
|
@param[in] p : instance of display
|
||||||
|
@param[in] data : image data (whole file)
|
||||||
|
@param[in] size : size of image data in bytes
|
||||||
|
@param[in] x_offset : offset of horizontal coordinate
|
||||||
|
@param[in] y_offset : offset of vertical coordinate
|
||||||
|
*/
|
||||||
|
void ssd1306_bmp_show_image_with_offset(ssd1306_t *p, const uint8_t *data, const long size, uint32_t x_offset, uint32_t y_offset);
|
||||||
|
|
||||||
|
/**
|
||||||
|
@brief draw monochrome bitmap
|
||||||
|
|
||||||
|
@param[in] p : instance of display
|
||||||
|
@param[in] data : image data (whole file)
|
||||||
|
@param[in] size : size of image data in bytes
|
||||||
|
*/
|
||||||
|
void ssd1306_bmp_show_image(ssd1306_t *p, const uint8_t *data, const long size);
|
||||||
|
|
||||||
|
/**
|
||||||
|
@brief draw char with given font
|
||||||
|
|
||||||
|
@param[in] p : instance of display
|
||||||
|
@param[in] x : x starting position of char
|
||||||
|
@param[in] y : y starting position of char
|
||||||
|
@param[in] scale : scale font to n times of original size (default should be 1)
|
||||||
|
@param[in] font : pointer to font
|
||||||
|
@param[in] c : character to draw
|
||||||
|
*/
|
||||||
|
void ssd1306_draw_char_with_font(ssd1306_t *p, uint32_t x, uint32_t y, uint32_t scale, const uint8_t *font, char c);
|
||||||
|
|
||||||
|
/**
|
||||||
|
@brief draw char with builtin font
|
||||||
|
|
||||||
|
@param[in] p : instance of display
|
||||||
|
@param[in] x : x starting position of char
|
||||||
|
@param[in] y : y starting position of char
|
||||||
|
@param[in] scale : scale font to n times of original size (default should be 1)
|
||||||
|
@param[in] c : character to draw
|
||||||
|
*/
|
||||||
|
void ssd1306_draw_char(ssd1306_t *p, uint32_t x, uint32_t y, uint32_t scale, char c);
|
||||||
|
|
||||||
|
/**
|
||||||
|
@brief draw string with given font
|
||||||
|
|
||||||
|
@param[in] p : instance of display
|
||||||
|
@param[in] x : x starting position of text
|
||||||
|
@param[in] y : y starting position of text
|
||||||
|
@param[in] scale : scale font to n times of original size (default should be 1)
|
||||||
|
@param[in] font : pointer to font
|
||||||
|
@param[in] s : text to draw
|
||||||
|
*/
|
||||||
|
void ssd1306_draw_string_with_font(ssd1306_t *p, uint32_t x, uint32_t y, uint32_t scale, const uint8_t *font, const char *s );
|
||||||
|
|
||||||
|
/**
|
||||||
|
@brief draw string with builtin font
|
||||||
|
|
||||||
|
@param[in] p : instance of display
|
||||||
|
@param[in] x : x starting position of text
|
||||||
|
@param[in] y : y starting position of text
|
||||||
|
@param[in] scale : scale font to n times of original size (default should be 1)
|
||||||
|
@param[in] s : text to draw
|
||||||
|
*/
|
||||||
|
void ssd1306_draw_string(ssd1306_t *p, uint32_t x, uint32_t y, uint32_t scale, const char *s);
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,200 @@
|
||||||
|
#include "include/io.h"
|
||||||
|
|
||||||
|
#include <pico/stdlib.h>
|
||||||
|
#include <time.h>
|
||||||
|
#include <hardware/pwm.h>
|
||||||
|
#include "include/gyro.h"
|
||||||
|
|
||||||
|
void motor_control_activated(bool activated)
|
||||||
|
{
|
||||||
|
const uint SLICE_NUM = pwm_gpio_to_slice_num(PIN_MOTOR1);
|
||||||
|
|
||||||
|
pwm_set_enabled(SLICE_NUM, activated);
|
||||||
|
}
|
||||||
|
|
||||||
|
static void init_tirette_and_color(void)
|
||||||
|
{
|
||||||
|
gpio_init(PIN_TIRETTE);
|
||||||
|
gpio_init(PIN_BUTTON_COLOR);
|
||||||
|
|
||||||
|
gpio_set_dir(PIN_TIRETTE, GPIO_OUT);
|
||||||
|
gpio_set_dir(PIN_BUTTON_COLOR, GPIO_OUT);
|
||||||
|
|
||||||
|
gpio_pull_up(PIN_TIRETTE);
|
||||||
|
gpio_pull_up(PIN_BUTTON_COLOR);
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
static void init_motors(void)
|
||||||
|
{
|
||||||
|
gpio_set_function(PIN_MOTOR1, GPIO_FUNC_PWM);
|
||||||
|
gpio_set_function(PIN_MOTOR2, GPIO_FUNC_PWM);
|
||||||
|
|
||||||
|
const uint SLICE_NUM = pwm_gpio_to_slice_num(PIN_MOTOR1);
|
||||||
|
pwm_set_wrap(SLICE_NUM, 4096);
|
||||||
|
|
||||||
|
pwm_set_gpio_level(PIN_MOTOR1, 0);
|
||||||
|
pwm_set_gpio_level(PIN_MOTOR2, 0);
|
||||||
|
|
||||||
|
pwm_set_enabled(SLICE_NUM, false);
|
||||||
|
}
|
||||||
|
|
||||||
|
static void init_servo(void)
|
||||||
|
{
|
||||||
|
const uint SLICE_NUM = pwm_gpio_to_slice_num(PIN_SERVO);
|
||||||
|
|
||||||
|
gpio_set_function(PIN_SERVO, GPIO_FUNC_PWM);
|
||||||
|
|
||||||
|
pwm_set_wrap(SLICE_NUM, 25000);
|
||||||
|
pwm_set_clkdiv(SLICE_NUM, 100);
|
||||||
|
|
||||||
|
pwm_set_gpio_level(PIN_SERVO, 12500);
|
||||||
|
|
||||||
|
pwm_set_enabled(SLICE_NUM, false);
|
||||||
|
}
|
||||||
|
|
||||||
|
static void init_i2c(void)
|
||||||
|
{
|
||||||
|
gpio_set_function(I2C_MASTER_SDA_PIN, GPIO_FUNC_I2C);
|
||||||
|
gpio_set_function(I2C_MASTER_SCL_PIN, GPIO_FUNC_I2C);
|
||||||
|
|
||||||
|
gpio_pull_up(I2C_MASTER_SDA_PIN);
|
||||||
|
gpio_pull_up(I2C_MASTER_SCL_PIN);
|
||||||
|
|
||||||
|
i2c_init(I2C_MASTER_INSTANCE, 100 * 1000);
|
||||||
|
}
|
||||||
|
|
||||||
|
static void init_screen(void)
|
||||||
|
{
|
||||||
|
io.screen->external_vcc=false;
|
||||||
|
ssd1306_init(io.screen, 128, 64, 0x3C, i2c1);
|
||||||
|
ssd1306_clear(io.screen);
|
||||||
|
ssd1306_show(io.screen);
|
||||||
|
}
|
||||||
|
|
||||||
|
void init_io(void)
|
||||||
|
{
|
||||||
|
init_tirette_and_color();
|
||||||
|
|
||||||
|
init_i2c();
|
||||||
|
|
||||||
|
init_gyro();
|
||||||
|
gyro_calibrate();
|
||||||
|
|
||||||
|
init_motors();
|
||||||
|
init_servo();
|
||||||
|
}
|
||||||
|
|
||||||
|
static void update_motion_control(void)
|
||||||
|
{
|
||||||
|
float actual_angle = io.gyro_data.z_angle;
|
||||||
|
float error = io.target_dir - actual_angle;
|
||||||
|
|
||||||
|
float correction = error * GAIN_KD;
|
||||||
|
|
||||||
|
int m1_speed = io.target_speed + correction;
|
||||||
|
int m2_speed = io.target_speed - correction;
|
||||||
|
|
||||||
|
if(m1_speed < 0)
|
||||||
|
{
|
||||||
|
m1_speed = 0;
|
||||||
|
}
|
||||||
|
else if(m2_speed < 0)
|
||||||
|
{
|
||||||
|
m2_speed = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
pwm_set_gpio_level(PIN_MOTOR1, m1_speed);
|
||||||
|
pwm_set_gpio_level(PIN_MOTOR2, m2_speed);
|
||||||
|
}
|
||||||
|
|
||||||
|
static void update_dancing_action(void)
|
||||||
|
{
|
||||||
|
static double prev_time = 0;
|
||||||
|
static double delta_time = 0;
|
||||||
|
static int actual_angle = 87;
|
||||||
|
static int8_t actual_dir = -1;
|
||||||
|
|
||||||
|
delta_time += io.time_ms - prev_time;
|
||||||
|
|
||||||
|
if(delta_time >= 10)
|
||||||
|
{
|
||||||
|
delta_time = 0;
|
||||||
|
|
||||||
|
actual_angle += actual_dir * DANCING_ACTION_DELTA_ANGLE;
|
||||||
|
}
|
||||||
|
|
||||||
|
if(actual_angle <= 43)
|
||||||
|
{
|
||||||
|
actual_dir = 1;
|
||||||
|
}
|
||||||
|
else if(actual_angle >= 130)
|
||||||
|
{
|
||||||
|
actual_dir = -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
pwm_set_gpio_level(PIN_SERVO, actual_angle);
|
||||||
|
|
||||||
|
prev_time = io.time_ms;
|
||||||
|
}
|
||||||
|
|
||||||
|
void update_io(void)
|
||||||
|
{
|
||||||
|
io.time_ms = (double)clock() * 1000.0 / CLOCKS_PER_SEC;
|
||||||
|
|
||||||
|
static double last_color_change_time = 0;
|
||||||
|
static bool has_color_changed = true;
|
||||||
|
static bool is_color_blue = true;
|
||||||
|
|
||||||
|
if(io.is_color_blue != is_color_blue)
|
||||||
|
{
|
||||||
|
ssd1306_clear(io.screen);
|
||||||
|
|
||||||
|
if(is_color_blue)
|
||||||
|
{
|
||||||
|
ssd1306_draw_string(io.screen, 0, 20, 1, "Blue\nTeam");
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
ssd1306_draw_string(io.screen, 0, 20, 1, "Yellow\nTeam");
|
||||||
|
}
|
||||||
|
|
||||||
|
last_color_change_time = io.time_ms;
|
||||||
|
|
||||||
|
has_color_changed = true;
|
||||||
|
}
|
||||||
|
/*
|
||||||
|
if(io.time_ms - last_color_change_time > 1000 && has_color_changed)
|
||||||
|
{
|
||||||
|
ssd1306_clear(io.screen);
|
||||||
|
m_screen.drawBitmap(38, 0, Res::Imgs::riombotique, 52, 64, WHITE);
|
||||||
|
//m_screen.drawBitmap(75, 0, Imgs::poivron_robotique, 52, 64, WHITE);
|
||||||
|
//m_screen.drawBitmap(27, 0, Res::Imgs::diable_gaga, 75, 64, WHITE);
|
||||||
|
|
||||||
|
has_color_changed = false;
|
||||||
|
}
|
||||||
|
*/
|
||||||
|
|
||||||
|
io.is_tirette_pulled = gpio_get(PIN_TIRETTE);
|
||||||
|
io.is_color_blue = is_color_blue;
|
||||||
|
|
||||||
|
is_color_blue = !gpio_get(PIN_BUTTON_COLOR);
|
||||||
|
|
||||||
|
if(io.motion_control_activated)
|
||||||
|
{
|
||||||
|
update_motion_control();
|
||||||
|
}
|
||||||
|
|
||||||
|
if(io.is_dancing)
|
||||||
|
{
|
||||||
|
update_dancing_action();
|
||||||
|
}
|
||||||
|
|
||||||
|
ssd1306_show(io.screen);
|
||||||
|
}
|
||||||
|
|
||||||
|
void set_dir_with_angular_speed(float dir)
|
||||||
|
{}
|
||||||
|
|
||||||
|
void io_deinit(void)
|
||||||
|
{}
|
||||||
145
Code/src/main.c
145
Code/src/main.c
|
|
@ -0,0 +1,145 @@
|
||||||
|
#include <pico/stdlib.h>
|
||||||
|
#include <stdio.h>
|
||||||
|
#include "include/io.h"
|
||||||
|
|
||||||
|
#define NORMAL_SPEED 2000.0f
|
||||||
|
#define TURNING_SPEED 1300.0f
|
||||||
|
#define ON_STAGE_SPEED 1100.0f
|
||||||
|
|
||||||
|
#define RAMPE_ANGLE -7.82907651006f
|
||||||
|
#define RAMPE_ANGLE_OFFSET 1.0f
|
||||||
|
#define RAMPE_START_ANGLE (RAMPE_ANGLE / 2 - RAMPE_ANGLE_OFFSET)
|
||||||
|
#define RAMPE_END_ANGLE (RAMPE_ANGLE / 2 + RAMPE_ANGLE_OFFSET)
|
||||||
|
|
||||||
|
// Unit tests activation
|
||||||
|
//#define UNIT_TESTS
|
||||||
|
|
||||||
|
/*
|
||||||
|
Etapes :
|
||||||
|
- 1ere etape : avancer jusqu'a la montée / condition : si montée detecté : etape suivante
|
||||||
|
- 2eme etape : avancer j'usqua la fin de la montée / condition : si fin de montée detecté : etape suivante
|
||||||
|
- 3eme etape : tourner de 90* / condition : si action terminé : etape suivante
|
||||||
|
- 4eme etape : avancer jusqu'au bors du plateau / condition : si choc de fin detecté : etape suivante
|
||||||
|
- 5eme etape : faire tourner actionneurs pour figurine
|
||||||
|
*/
|
||||||
|
|
||||||
|
/*
|
||||||
|
Modules necessaires :
|
||||||
|
- angle // Asservissement et control du robot
|
||||||
|
- choc // Controle du robot (fin !)
|
||||||
|
(?)- accelerometre // Asservissement
|
||||||
|
*/
|
||||||
|
|
||||||
|
// Movement states
|
||||||
|
typedef enum state_t {
|
||||||
|
WaitingForTirette,
|
||||||
|
WaitingTimer,
|
||||||
|
ForwardToRamp,
|
||||||
|
ForwardToScene,
|
||||||
|
Turn90Blue, // If blue team
|
||||||
|
Turn90Yellow, // If yellow team => this is the only action where you have to do something different depending on your team
|
||||||
|
ForwardToSceneEdge,
|
||||||
|
Dancing
|
||||||
|
} state_t;
|
||||||
|
|
||||||
|
io_t io;
|
||||||
|
|
||||||
|
void main(void)
|
||||||
|
{
|
||||||
|
stdio_init_all();
|
||||||
|
|
||||||
|
// Initialise IO
|
||||||
|
init_io();
|
||||||
|
|
||||||
|
motor_control_activated(true);
|
||||||
|
|
||||||
|
io.target_dir = 0.0f;
|
||||||
|
io.target_speed = 0.0f;
|
||||||
|
//my_IO.startDancingAction(15);
|
||||||
|
|
||||||
|
#ifdef UNIT_TESTS
|
||||||
|
Serial.println("UNIT_TESTS");
|
||||||
|
#endif
|
||||||
|
|
||||||
|
printf("Rampe Angle : %f\n", RAMPE_ANGLE);
|
||||||
|
printf("Rampe Angle Offset : %f\n", RAMPE_ANGLE_OFFSET);
|
||||||
|
printf("Rampe Start Angle : %f\n", RAMPE_START_ANGLE);
|
||||||
|
printf("Rampe End Angle : %f\n", RAMPE_END_ANGLE);
|
||||||
|
|
||||||
|
state_t actual_state = WaitingForTirette;
|
||||||
|
|
||||||
|
while(true)
|
||||||
|
{
|
||||||
|
update_io();
|
||||||
|
|
||||||
|
switch(actual_state)
|
||||||
|
{
|
||||||
|
case WaitingForTirette:
|
||||||
|
if(io.is_tirette_pulled)
|
||||||
|
{
|
||||||
|
actual_state = WaitingTimer;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case WaitingTimer:
|
||||||
|
unsigned long initial_time = io.time_ms;
|
||||||
|
|
||||||
|
if(io.time_ms - initial_time >= 87000)
|
||||||
|
{
|
||||||
|
actual_state = ForwardToRamp;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case ForwardToRamp:
|
||||||
|
motor_control_activated(true);
|
||||||
|
io.target_speed = NORMAL_SPEED;
|
||||||
|
|
||||||
|
if(io.gyro_data.y_angle < RAMPE_START_ANGLE)
|
||||||
|
{
|
||||||
|
actual_state = ForwardToScene;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case ForwardToScene:
|
||||||
|
if(io.gyro_data.y_angle > RAMPE_END_ANGLE)
|
||||||
|
{
|
||||||
|
io.target_speed = TURNING_SPEED;
|
||||||
|
actual_state = io.is_color_blue ? Turn90Blue : Turn90Yellow;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case Turn90Blue:
|
||||||
|
set_dir_with_angular_speed(90.0f);
|
||||||
|
|
||||||
|
if(io.gyro_data.z_angle >= 80.0f)
|
||||||
|
{
|
||||||
|
actual_state = ForwardToSceneEdge;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case Turn90Yellow:
|
||||||
|
set_dir_with_angular_speed(-90.0f);
|
||||||
|
|
||||||
|
if(io.gyro_data.z_angle <= -80.0f)
|
||||||
|
{
|
||||||
|
actual_state = ForwardToSceneEdge;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case ForwardToSceneEdge:
|
||||||
|
io.target_speed = ON_STAGE_SPEED;
|
||||||
|
|
||||||
|
if(io.gyro_data.y_angle > 3.0f)
|
||||||
|
{
|
||||||
|
actual_state = Dancing;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case Dancing:
|
||||||
|
motor_control_activated(false);
|
||||||
|
io.target_speed = 0.0f;
|
||||||
|
io.is_dancing = true;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,306 @@
|
||||||
|
/*
|
||||||
|
|
||||||
|
MIT License
|
||||||
|
|
||||||
|
Copyright (c) 2021 David Schramm
|
||||||
|
|
||||||
|
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||||
|
of this software and associated documentation files (the "Software"), to deal
|
||||||
|
in the Software without restriction, including without limitation the rights
|
||||||
|
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||||
|
copies of the Software, and to permit persons to whom the Software is
|
||||||
|
furnished to do so, subject to the following conditions:
|
||||||
|
|
||||||
|
The above copyright notice and this permission notice shall be included in all
|
||||||
|
copies or substantial portions of the Software.
|
||||||
|
|
||||||
|
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||||
|
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||||
|
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||||
|
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||||
|
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||||
|
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||||
|
SOFTWARE.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <pico/stdlib.h>
|
||||||
|
#include <hardware/i2c.h>
|
||||||
|
#include <pico/binary_info.h>
|
||||||
|
#include <stdlib.h>
|
||||||
|
#include <string.h>
|
||||||
|
#include <stdio.h>
|
||||||
|
|
||||||
|
#include "include/ssd1306.h"
|
||||||
|
#include "include/font.h"
|
||||||
|
|
||||||
|
inline static void swap(int32_t *a, int32_t *b) {
|
||||||
|
int32_t *t=a;
|
||||||
|
*a=*b;
|
||||||
|
*b=*t;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline static void fancy_write(i2c_inst_t *i2c, uint8_t addr, const uint8_t *src, size_t len, char *name) {
|
||||||
|
switch(i2c_write_blocking(i2c, addr, src, len, false)) {
|
||||||
|
case PICO_ERROR_GENERIC:
|
||||||
|
printf("[%s] addr not acknowledged!\n", name);
|
||||||
|
break;
|
||||||
|
case PICO_ERROR_TIMEOUT:
|
||||||
|
printf("[%s] timeout!\n", name);
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
//printf("[%s] wrote successfully %lu bytes!\n", name, len);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
inline static void ssd1306_write(ssd1306_t *p, uint8_t val) {
|
||||||
|
uint8_t d[2]= {0x00, val};
|
||||||
|
fancy_write(p->i2c_i, p->address, d, 2, "ssd1306_write");
|
||||||
|
}
|
||||||
|
|
||||||
|
bool ssd1306_init(ssd1306_t *p, uint16_t width, uint16_t height, uint8_t address, i2c_inst_t *i2c_instance) {
|
||||||
|
p->width=width;
|
||||||
|
p->height=height;
|
||||||
|
p->pages=height/8;
|
||||||
|
p->address=address;
|
||||||
|
|
||||||
|
p->i2c_i=i2c_instance;
|
||||||
|
|
||||||
|
|
||||||
|
p->bufsize=(p->pages)*(p->width);
|
||||||
|
if((p->buffer=malloc(p->bufsize+1))==NULL) {
|
||||||
|
p->bufsize=0;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
++(p->buffer);
|
||||||
|
|
||||||
|
// from https://github.com/makerportal/rpi-pico-ssd1306
|
||||||
|
uint8_t cmds[]= {
|
||||||
|
SET_DISP,
|
||||||
|
// timing and driving scheme
|
||||||
|
SET_DISP_CLK_DIV,
|
||||||
|
0x80,
|
||||||
|
SET_MUX_RATIO,
|
||||||
|
height - 1,
|
||||||
|
SET_DISP_OFFSET,
|
||||||
|
0x00,
|
||||||
|
// resolution and layout
|
||||||
|
SET_DISP_START_LINE,
|
||||||
|
// charge pump
|
||||||
|
SET_CHARGE_PUMP,
|
||||||
|
p->external_vcc?0x10:0x14,
|
||||||
|
SET_SEG_REMAP | 0x01, // column addr 127 mapped to SEG0
|
||||||
|
SET_COM_OUT_DIR | 0x08, // scan from COM[N] to COM0
|
||||||
|
SET_COM_PIN_CFG,
|
||||||
|
width>2*height?0x02:0x12,
|
||||||
|
// display
|
||||||
|
SET_CONTRAST,
|
||||||
|
0xff,
|
||||||
|
SET_PRECHARGE,
|
||||||
|
p->external_vcc?0x22:0xF1,
|
||||||
|
SET_VCOM_DESEL,
|
||||||
|
0x30, // or 0x40?
|
||||||
|
SET_ENTIRE_ON, // output follows RAM contents
|
||||||
|
SET_NORM_INV, // not inverted
|
||||||
|
SET_DISP | 0x01,
|
||||||
|
// address setting
|
||||||
|
SET_MEM_ADDR,
|
||||||
|
0x00, // horizontal
|
||||||
|
};
|
||||||
|
|
||||||
|
for(size_t i=0; i<sizeof(cmds); ++i)
|
||||||
|
ssd1306_write(p, cmds[i]);
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void ssd1306_deinit(ssd1306_t *p) {
|
||||||
|
free(p->buffer-1);
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void ssd1306_poweroff(ssd1306_t *p) {
|
||||||
|
ssd1306_write(p, SET_DISP|0x00);
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void ssd1306_poweron(ssd1306_t *p) {
|
||||||
|
ssd1306_write(p, SET_DISP|0x01);
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void ssd1306_contrast(ssd1306_t *p, uint8_t val) {
|
||||||
|
ssd1306_write(p, SET_CONTRAST);
|
||||||
|
ssd1306_write(p, val);
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void ssd1306_invert(ssd1306_t *p, uint8_t inv) {
|
||||||
|
ssd1306_write(p, SET_NORM_INV | (inv & 1));
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void ssd1306_clear(ssd1306_t *p) {
|
||||||
|
memset(p->buffer, 0, p->bufsize);
|
||||||
|
}
|
||||||
|
|
||||||
|
void ssd1306_clear_pixel(ssd1306_t *p, uint32_t x, uint32_t y) {
|
||||||
|
if(x>=p->width || y>=p->height) return;
|
||||||
|
|
||||||
|
p->buffer[x+p->width*(y>>3)]&=~(0x1<<(y&0x07));
|
||||||
|
}
|
||||||
|
|
||||||
|
void ssd1306_draw_pixel(ssd1306_t *p, uint32_t x, uint32_t y) {
|
||||||
|
if(x>=p->width || y>=p->height) return;
|
||||||
|
|
||||||
|
p->buffer[x+p->width*(y>>3)]|=0x1<<(y&0x07); // y>>3==y/8 && y&0x7==y%8
|
||||||
|
}
|
||||||
|
|
||||||
|
void ssd1306_draw_line(ssd1306_t *p, int32_t x1, int32_t y1, int32_t x2, int32_t y2) {
|
||||||
|
if(x1>x2) {
|
||||||
|
swap(&x1, &x2);
|
||||||
|
swap(&y1, &y2);
|
||||||
|
}
|
||||||
|
|
||||||
|
if(x1==x2) {
|
||||||
|
if(y1>y2)
|
||||||
|
swap(&y1, &y2);
|
||||||
|
for(int32_t i=y1; i<=y2; ++i)
|
||||||
|
ssd1306_draw_pixel(p, x1, i);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
float m=(float) (y2-y1) / (float) (x2-x1);
|
||||||
|
|
||||||
|
for(int32_t i=x1; i<=x2; ++i) {
|
||||||
|
float y=m*(float) (i-x1)+(float) y1;
|
||||||
|
ssd1306_draw_pixel(p, i, (uint32_t) y);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void ssd1306_clear_square(ssd1306_t *p, uint32_t x, uint32_t y, uint32_t width, uint32_t height) {
|
||||||
|
for(uint32_t i=0; i<width; ++i)
|
||||||
|
for(uint32_t j=0; j<height; ++j)
|
||||||
|
ssd1306_clear_pixel(p, x+i, y+j);
|
||||||
|
}
|
||||||
|
|
||||||
|
void ssd1306_draw_square(ssd1306_t *p, uint32_t x, uint32_t y, uint32_t width, uint32_t height) {
|
||||||
|
for(uint32_t i=0; i<width; ++i)
|
||||||
|
for(uint32_t j=0; j<height; ++j)
|
||||||
|
ssd1306_draw_pixel(p, x+i, y+j);
|
||||||
|
}
|
||||||
|
|
||||||
|
void ssd1306_draw_empty_square(ssd1306_t *p, uint32_t x, uint32_t y, uint32_t width, uint32_t height) {
|
||||||
|
ssd1306_draw_line(p, x, y, x+width, y);
|
||||||
|
ssd1306_draw_line(p, x, y+height, x+width, y+height);
|
||||||
|
ssd1306_draw_line(p, x, y, x, y+height);
|
||||||
|
ssd1306_draw_line(p, x+width, y, x+width, y+height);
|
||||||
|
}
|
||||||
|
|
||||||
|
void ssd1306_draw_char_with_font(ssd1306_t *p, uint32_t x, uint32_t y, uint32_t scale, const uint8_t *font, char c) {
|
||||||
|
if(c<font[3]||c>font[4])
|
||||||
|
return;
|
||||||
|
|
||||||
|
uint32_t parts_per_line=(font[0]>>3)+((font[0]&7)>0);
|
||||||
|
for(uint8_t w=0; w<font[1]; ++w) { // width
|
||||||
|
uint32_t pp=(c-font[3])*font[1]*parts_per_line+w*parts_per_line+5;
|
||||||
|
for(uint32_t lp=0; lp<parts_per_line; ++lp) {
|
||||||
|
uint8_t line=font[pp];
|
||||||
|
|
||||||
|
for(int8_t j=0; j<8; ++j, line>>=1) {
|
||||||
|
if(line & 1)
|
||||||
|
ssd1306_draw_square(p, x+w*scale, y+((lp<<3)+j)*scale, scale, scale);
|
||||||
|
}
|
||||||
|
|
||||||
|
++pp;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void ssd1306_draw_string_with_font(ssd1306_t *p, uint32_t x, uint32_t y, uint32_t scale, const uint8_t *font, const char *s) {
|
||||||
|
for(int32_t x_n=x; *s; x_n+=(font[1]+font[2])*scale) {
|
||||||
|
ssd1306_draw_char_with_font(p, x_n, y, scale, font, *(s++));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void ssd1306_draw_char(ssd1306_t *p, uint32_t x, uint32_t y, uint32_t scale, char c) {
|
||||||
|
ssd1306_draw_char_with_font(p, x, y, scale, font_8x5, c);
|
||||||
|
}
|
||||||
|
|
||||||
|
void ssd1306_draw_string(ssd1306_t *p, uint32_t x, uint32_t y, uint32_t scale, const char *s) {
|
||||||
|
ssd1306_draw_string_with_font(p, x, y, scale, font_8x5, s);
|
||||||
|
}
|
||||||
|
|
||||||
|
static inline uint32_t ssd1306_bmp_get_val(const uint8_t *data, const size_t offset, uint8_t size) {
|
||||||
|
switch(size) {
|
||||||
|
case 1:
|
||||||
|
return data[offset];
|
||||||
|
case 2:
|
||||||
|
return data[offset]|(data[offset+1]<<8);
|
||||||
|
case 4:
|
||||||
|
return data[offset]|(data[offset+1]<<8)|(data[offset+2]<<16)|(data[offset+3]<<24);
|
||||||
|
default:
|
||||||
|
__builtin_unreachable();
|
||||||
|
}
|
||||||
|
__builtin_unreachable();
|
||||||
|
}
|
||||||
|
|
||||||
|
void ssd1306_bmp_show_image_with_offset(ssd1306_t *p, const uint8_t *data, const long size, uint32_t x_offset, uint32_t y_offset) {
|
||||||
|
if(size<54) // data smaller than header
|
||||||
|
return;
|
||||||
|
|
||||||
|
const uint32_t bfOffBits=ssd1306_bmp_get_val(data, 10, 4);
|
||||||
|
const uint32_t biSize=ssd1306_bmp_get_val(data, 14, 4);
|
||||||
|
const uint32_t biWidth=ssd1306_bmp_get_val(data, 18, 4);
|
||||||
|
const int32_t biHeight=(int32_t) ssd1306_bmp_get_val(data, 22, 4);
|
||||||
|
const uint16_t biBitCount=(uint16_t) ssd1306_bmp_get_val(data, 28, 2);
|
||||||
|
const uint32_t biCompression=ssd1306_bmp_get_val(data, 30, 4);
|
||||||
|
|
||||||
|
if(biBitCount!=1) // image not monochrome
|
||||||
|
return;
|
||||||
|
|
||||||
|
if(biCompression!=0) // image compressed
|
||||||
|
return;
|
||||||
|
|
||||||
|
const int table_start=14+biSize;
|
||||||
|
uint8_t color_val=0;
|
||||||
|
|
||||||
|
for(uint8_t i=0; i<2; ++i) {
|
||||||
|
if(!((data[table_start+i*4]<<16)|(data[table_start+i*4+1]<<8)|data[table_start+i*4+2])) {
|
||||||
|
color_val=i;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
uint32_t bytes_per_line=(biWidth/8)+(biWidth&7?1:0);
|
||||||
|
if(bytes_per_line&3)
|
||||||
|
bytes_per_line=(bytes_per_line^(bytes_per_line&3))+4;
|
||||||
|
|
||||||
|
const uint8_t *img_data=data+bfOffBits;
|
||||||
|
|
||||||
|
int32_t step=biHeight>0?-1:1;
|
||||||
|
int32_t border=biHeight>0?-1:-biHeight;
|
||||||
|
|
||||||
|
for(uint32_t y=biHeight>0?biHeight-1:0; y!=(uint32_t)border; y+=step) {
|
||||||
|
for(uint32_t x=0; x<biWidth; ++x) {
|
||||||
|
if(((img_data[x>>3]>>(7-(x&7)))&1)==color_val)
|
||||||
|
ssd1306_draw_pixel(p, x_offset+x, y_offset+y);
|
||||||
|
}
|
||||||
|
img_data+=bytes_per_line;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void ssd1306_bmp_show_image(ssd1306_t *p, const uint8_t *data, const long size) {
|
||||||
|
ssd1306_bmp_show_image_with_offset(p, data, size, 0, 0);
|
||||||
|
}
|
||||||
|
|
||||||
|
void ssd1306_show(ssd1306_t *p) {
|
||||||
|
uint8_t payload[]= {SET_COL_ADDR, 0, p->width-1, SET_PAGE_ADDR, 0, p->pages-1};
|
||||||
|
if(p->width==64) {
|
||||||
|
payload[1]+=32;
|
||||||
|
payload[2]+=32;
|
||||||
|
}
|
||||||
|
|
||||||
|
for(size_t i=0; i<sizeof(payload); ++i)
|
||||||
|
ssd1306_write(p, payload[i]);
|
||||||
|
|
||||||
|
*(p->buffer-1)=0x40;
|
||||||
|
|
||||||
|
fancy_write(p->i2c_i, p->address, p->buffer-1, p->bufsize+1, "ssd1306_show");
|
||||||
|
}
|
||||||
Loading…
Reference in New Issue