diff --git a/Code/.gitignore b/Code/.gitignore new file mode 100644 index 0000000..567609b --- /dev/null +++ b/Code/.gitignore @@ -0,0 +1 @@ +build/ diff --git a/Code/CMakeLists.txt b/Code/CMakeLists.txt index bcce13b..cfb1c61 100644 --- a/Code/CMakeLists.txt +++ b/Code/CMakeLists.txt @@ -12,13 +12,9 @@ pico_sdk_init() add_executable(motion_controller src/main.c - src/robot.c - src/motors.c src/gyro.c - src/motion_control.c - src/i2c_master.c - src/udp_client.c - src/udp_buffer.c + src/ssd1306.c + src/io.c ) target_link_libraries(motion_controller diff --git a/Code/src/gyro.c b/Code/src/gyro.c new file mode 100644 index 0000000..9257f2d --- /dev/null +++ b/Code/src/gyro.c @@ -0,0 +1,137 @@ +#include "include/gyro.h" + +#include +#include +#include "include/io.h" + +//#include + +/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *\ +* 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; + } +} diff --git a/Code/src/include/font.h b/Code/src/include/font.h new file mode 100644 index 0000000..8c9ff4a --- /dev/null +++ b/Code/src/include/font.h @@ -0,0 +1,110 @@ +#ifndef _inc_font +#define _inc_font + +/* + * Format + * , , , + * , , + * + */ +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 diff --git a/Code/src/include/gyro.h b/Code/src/include/gyro.h new file mode 100644 index 0000000..77c7faf --- /dev/null +++ b/Code/src/include/gyro.h @@ -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 \ No newline at end of file diff --git a/Code/src/include/images.h b/Code/src/include/images.h new file mode 100644 index 0000000..e69de29 diff --git a/Code/src/include/io.h b/Code/src/include/io.h new file mode 100644 index 0000000..61bada5 --- /dev/null +++ b/Code/src/include/io.h @@ -0,0 +1,53 @@ +#ifndef IO_H +#define IO_H + +#include +#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 \ No newline at end of file diff --git a/Code/src/include/ssd1306.h b/Code/src/include/ssd1306.h new file mode 100644 index 0000000..e9b8f61 --- /dev/null +++ b/Code/src/include/ssd1306.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 + +/** +* @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 diff --git a/Code/src/io.c b/Code/src/io.c new file mode 100644 index 0000000..417c703 --- /dev/null +++ b/Code/src/io.c @@ -0,0 +1,200 @@ +#include "include/io.h" + +#include +#include +#include +#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) +{} diff --git a/Code/src/main.c b/Code/src/main.c index e69de29..4d93682 100644 --- a/Code/src/main.c +++ b/Code/src/main.c @@ -0,0 +1,145 @@ +#include +#include +#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; + } + } +} diff --git a/Code/src/ssd1306.c b/Code/src/ssd1306.c new file mode 100644 index 0000000..2f54d79 --- /dev/null +++ b/Code/src/ssd1306.c @@ -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 +#include +#include +#include +#include +#include + +#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; ibuffer-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; ifont[4]) + return; + + uint32_t parts_per_line=(font[0]>>3)+((font[0]&7)>0); + for(uint8_t w=0; w>=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>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; ibuffer-1)=0x40; + + fancy_write(p->i2c_i, p->address, p->buffer-1, p->bufsize+1, "ssd1306_show"); +}