Started main controller code (copied useful files from motion controller)
This commit is contained in:
parent
cb7188c751
commit
5e32b7f950
|
|
@ -0,0 +1,31 @@
|
|||
{
|
||||
"env": {
|
||||
"myDefaultIncludePath": [
|
||||
"${env:PICO_SDK_PATH}/src/**/include/",
|
||||
"${workspaceFolder}/build/generated/pico_base/"
|
||||
],
|
||||
"myCompilerPath": "/usr/bin/arm-none-eabi-gcc"
|
||||
},
|
||||
|
||||
"configurations": [
|
||||
{
|
||||
"name": "Linux",
|
||||
"intelliSenseMode": "linux-gcc-arm",
|
||||
"includePath": [
|
||||
"${myDefaultIncludePath}"
|
||||
],
|
||||
"compilerPath": "/usr/bin/arm-none-eabi-gcc",
|
||||
"cStandard": "c11",
|
||||
"cppStandard": "c++17",
|
||||
"browse": {
|
||||
"path": [
|
||||
"${workspaceFolder}"
|
||||
],
|
||||
"limitSymbolsToIncludedHeaders": true,
|
||||
"databaseFilename": ""
|
||||
},
|
||||
"configurationProvider": "ms-vscode.cmake-tools"
|
||||
}
|
||||
],
|
||||
"version": 4
|
||||
}
|
||||
|
|
@ -0,0 +1,7 @@
|
|||
{
|
||||
"files.associations": {
|
||||
"binary_info.h": "c",
|
||||
"i2c.h": "c",
|
||||
"stdlib.h": "c"
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,25 @@
|
|||
{
|
||||
"tasks": [
|
||||
{
|
||||
"type": "shell",
|
||||
"command": "cd build; cmake ../; make",
|
||||
"label": "CMake in build/",
|
||||
"problemMatcher": [],
|
||||
"group": {
|
||||
"kind": "build",
|
||||
"isDefault": false
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "shell",
|
||||
"command": "cd build; cmake ../; make Flash",
|
||||
"label": "CMake & Make & Flash",
|
||||
"problemMatcher": [],
|
||||
"group": {
|
||||
"kind": "build",
|
||||
"isDefault": true
|
||||
}
|
||||
}
|
||||
],
|
||||
"version": "2.0.0"
|
||||
}
|
||||
|
|
@ -0,0 +1,32 @@
|
|||
cmake_minimum_required(VERSION 3.13)
|
||||
|
||||
include(pico_sdk_import.cmake)
|
||||
|
||||
project(motion_controller C CXX ASM)
|
||||
set(CMAKE_C_STNDARD 11)
|
||||
set(CMAKE_CXX_STANDARD 17)
|
||||
|
||||
set(PICO_EXAMPLES_PATH ${PROJECT_SOURCE_DIR})
|
||||
|
||||
pico_sdk_init()
|
||||
|
||||
add_executable(motion_controller
|
||||
src/main.c
|
||||
)
|
||||
|
||||
target_link_libraries(motion_controller
|
||||
hardware_i2c
|
||||
hardware_pwm
|
||||
hardware_uart
|
||||
pico_stdlib
|
||||
)
|
||||
|
||||
pico_enable_stdio_usb(motion_controller 1)
|
||||
pico_enable_stdio_uart(motion_controller 1)
|
||||
|
||||
pico_add_extra_outputs(motion_controller)
|
||||
|
||||
add_custom_target(Flash
|
||||
DEPENDS motion_controller
|
||||
COMMAND sudo picotool load -f ${PROJECT_BINARY_DIR}/motion_controller.uf2
|
||||
)
|
||||
|
|
@ -0,0 +1,54 @@
|
|||
Motion controler code for the RPI Pico (RP2040)
|
||||
===============================================
|
||||
|
||||
This project is the main controller firmware for the RPI Pico (RP2040), designed for the Eurobot 2025 Cup.
|
||||
|
||||
|
||||
I2C description
|
||||
-----------------------------------------------
|
||||
|
||||
The robot’s I2C communication works as follows:
|
||||
* Send the device address + R/W bit (to select read or write mode).
|
||||
* Send the target register address (to read from or write to).
|
||||
* Read or write the register data. Multiple registers can be read/written sequentially, with the address auto-incrementing after each operation.
|
||||
|
||||
This code is designed to be the master in the i2c communication;
|
||||
|
||||
|Adress |R/W|Description |Encoding |
|
||||
|-------|:-:|-------------------------------|:-----------------:|
|
||||
| 0x00 | W | Speed motor 1 |**-128** - **127** |
|
||||
| 0x01 | W | Speed motor 2 |**-128** - **127** |
|
||||
| 0x02 | W | Speed motor 3 |**-128** - **127** |
|
||||
| 0x03 | W | Speed motor 4 |**-128** - **127** |
|
||||
| 0x04 | W | Servo 1 position selection | **0** - **1** |
|
||||
| 0x05 | W | Servo 2 position selection | **0** - **1** |
|
||||
| 0x06 | W | Servo 3 position selection | **0** - **1** |
|
||||
| 0x07 | W | Servo 4 position selection | **0** - **1** |
|
||||
|
||||
|
||||
Motors communication description
|
||||
-----------------------------------------------
|
||||
|
||||
Motors are «connected» to their respective I2C buffer address.
|
||||
|
||||
To control a motor you need to write data to its adress of the form :
|
||||
|
||||
>```C
|
||||
>int8_t speed;
|
||||
>```
|
||||
|
||||
Value goes from **-128** to **127**.
|
||||
|
||||
|
||||
Servo motors communication description
|
||||
-----------------------------------------------
|
||||
|
||||
Servo motors are «connected» to their respective I2C buffer address.
|
||||
|
||||
To control a servo motor you need to write data to its adress of the form :
|
||||
|
||||
>```C
|
||||
>uint8_t close;
|
||||
>```
|
||||
|
||||
Value is 0 or 1 for the open or the close pos.
|
||||
|
|
@ -0,0 +1,107 @@
|
|||
/*****
|
||||
*
|
||||
* Le principe est que la télécommande soit l'esclave I2C
|
||||
* Pour envoyer un message, on charge le message à l'adresse 0 de la "mémoire" pour l'i2c
|
||||
*
|
||||
* Pour lire le message, le robot interroge la télécommande, et demande le contenu à partir de l'adresse 0 de la mémoire.
|
||||
* Ainsi, en cas d'échec de la communication, le robot détectera une manette débranchée.
|
||||
* Par défaut, la fonction lit 255 caractères.
|
||||
*
|
||||
* Copyright (c) 2024 - Club robotique de Riom
|
||||
*
|
||||
*
|
||||
* SPDX-License-Identifier: BSD-3-Clause
|
||||
*/
|
||||
#include "i2c_fifo.h"
|
||||
#include "i2c_slave.h"
|
||||
#include "i2c_maitre.h"
|
||||
#include "string.h"
|
||||
|
||||
|
||||
/// DEBUT DE LA CONFIGURATION de L'I2C
|
||||
|
||||
#define I2C0_SDA_PIN 16
|
||||
#define I2C0_SCL_PIN 17
|
||||
|
||||
#define I2C_SLAVE_ADDRESS 0x17
|
||||
|
||||
static const uint I2C_SLAVE_SDA_PIN = I2C0_SDA_PIN;
|
||||
static const uint I2C_SLAVE_SCL_PIN = I2C0_SCL_PIN;
|
||||
|
||||
// The slave implements a 256 byte memory. To write a series of bytes, the master first
|
||||
// writes the memory address, followed by the data. The address is automatically incremented
|
||||
// for each byte transferred, looping back to 0 upon reaching the end. Reading is done
|
||||
// sequentially from the current memory address.
|
||||
static struct
|
||||
{
|
||||
uint8_t mem[256];
|
||||
uint8_t mem_address;
|
||||
bool mem_address_written;
|
||||
} context;
|
||||
|
||||
// Our handler is called from the I2C ISR, so it must complete quickly. Blocking calls /
|
||||
// printing to stdio may interfere with interrupt handling.
|
||||
static void i2c_slave_handler(i2c_inst_t *i2c, i2c_slave_event_t event) {
|
||||
switch (event) {
|
||||
case I2C_SLAVE_RECEIVE: // master has written some data
|
||||
if (!context.mem_address_written) {
|
||||
// writes always start with the memory address
|
||||
context.mem_address = i2c_read_byte(i2c);
|
||||
context.mem_address_written = true;
|
||||
} else {
|
||||
// save into memory
|
||||
context.mem[context.mem_address] = i2c_read_byte(i2c);
|
||||
context.mem_address++;
|
||||
}
|
||||
break;
|
||||
case I2C_SLAVE_REQUEST: // master is requesting data
|
||||
// load from memory
|
||||
i2c_write_byte(i2c, context.mem[context.mem_address]);
|
||||
context.mem_address++;
|
||||
break;
|
||||
case I2C_SLAVE_FINISH: // master has signalled Stop / Restart
|
||||
context.mem_address_written = false;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void i2c_set_slave_mode_perso(i2c_inst_t *i2c, uint8_t addr) {
|
||||
i2c->hw->enable = 0;
|
||||
|
||||
//while( !(i2c->hw->enable_status & 0x1) );
|
||||
|
||||
i2c->hw->sar = addr;
|
||||
i2c->hw->con = 0;
|
||||
|
||||
i2c->hw->enable = 1;
|
||||
}
|
||||
|
||||
static void setup_slave() {
|
||||
gpio_init(I2C_SLAVE_SDA_PIN);
|
||||
gpio_set_function(I2C_SLAVE_SDA_PIN, GPIO_FUNC_I2C);
|
||||
gpio_pull_up(I2C_SLAVE_SDA_PIN);
|
||||
|
||||
gpio_init(I2C_SLAVE_SCL_PIN);
|
||||
gpio_set_function(I2C_SLAVE_SCL_PIN, GPIO_FUNC_I2C);
|
||||
gpio_pull_up(I2C_SLAVE_SCL_PIN);
|
||||
|
||||
i2c_slave_init(i2c0, I2C_SLAVE_ADDRESS, &i2c_slave_handler);
|
||||
}
|
||||
/// FIN DE LA CONFIGURATION de L'I2C
|
||||
|
||||
|
||||
void communication_init(void){
|
||||
setup_slave();
|
||||
i2c_maitre_init();
|
||||
}
|
||||
|
||||
void communication_envoyer_message(unsigned char * message, unsigned int message_length){
|
||||
memcpy(context.mem, message, message_length);
|
||||
}
|
||||
|
||||
int communication_lire_message(unsigned char * message){
|
||||
i2c_lire_registre(I2C_SLAVE_ADDRESS, 0, message, 255);
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,5 @@
|
|||
#include "i2c_maitre.h"
|
||||
|
||||
void communication_init(void);
|
||||
void communication_envoyer_message(unsigned char * message, unsigned int message_length);
|
||||
enum i2c_resultat_t communication_lire_message(unsigned char * message);
|
||||
|
|
@ -0,0 +1,53 @@
|
|||
/*
|
||||
* Copyright (c) 2021 Valentin Milea <valentin.milea@gmail.com>
|
||||
*
|
||||
* SPDX-License-Identifier: MIT
|
||||
*/
|
||||
|
||||
#ifndef _I2C_FIFO_H_
|
||||
#define _I2C_FIFO_H_
|
||||
|
||||
#include <hardware/i2c.h>
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/** \file i2c_fifo.h
|
||||
*
|
||||
* \brief I2C non-blocking r/w.
|
||||
*/
|
||||
|
||||
/**
|
||||
* \brief Pop a byte from I2C Rx FIFO.
|
||||
*
|
||||
* This function is non-blocking and assumes the Rx FIFO isn't empty.
|
||||
*
|
||||
* \param i2c I2C instance.
|
||||
* \return uint8_t Byte value.
|
||||
*/
|
||||
static inline uint8_t i2c_read_byte(i2c_inst_t *i2c) {
|
||||
i2c_hw_t *hw = i2c_get_hw(i2c);
|
||||
assert(hw->status & I2C_IC_STATUS_RFNE_BITS); // Rx FIFO must not be empty
|
||||
return (uint8_t)hw->data_cmd;
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief Push a byte into I2C Tx FIFO.
|
||||
*
|
||||
* This function is non-blocking and assumes the Tx FIFO isn't full.
|
||||
*
|
||||
* \param i2c I2C instance.
|
||||
* \param value Byte value.
|
||||
*/
|
||||
static inline void i2c_write_byte(i2c_inst_t *i2c, uint8_t value) {
|
||||
i2c_hw_t *hw = i2c_get_hw(i2c);
|
||||
assert(hw->status & I2C_IC_STATUS_TFNF_BITS); // Tx FIFO must not be full
|
||||
hw->data_cmd = value;
|
||||
}
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // _I2C_FIFO_H_
|
||||
|
|
@ -0,0 +1,281 @@
|
|||
#include "i2c_maitre.h"
|
||||
#include "hardware/gpio.h"
|
||||
#include "hardware/i2c.h"
|
||||
#include "pico/stdlib.h"
|
||||
#include <stdio.h>
|
||||
|
||||
#define I2C1_SDA_PIN 18
|
||||
#define I2C1_SCL_PIN 19
|
||||
|
||||
#define I2C_NB_MAX_TAMPON 20
|
||||
|
||||
enum i2c_statu_t{
|
||||
I2C_STATU_LIBRE,
|
||||
I2C_STATU_OCCUPE
|
||||
} i2c_statu_i2c1;
|
||||
|
||||
uint16_t I2C_tampon_envoi[I2C_NB_MAX_TAMPON];
|
||||
uint8_t I2C_tampon_reception[I2C_NB_MAX_TAMPON];
|
||||
uint16_t I2C_nb_a_envoyer, I2C_nb_a_recevoir;
|
||||
uint8_t adresse_7_bits;
|
||||
uint32_t i2c_error_code; // value of i2c->hw->tx_abrt_source if anything wrong happen, 0 if everything was fine.
|
||||
|
||||
enum transaction_statu_t{
|
||||
TRANSACTION_EN_COURS,
|
||||
TRANSACTION_TERMINEE
|
||||
} statu_emission, statu_reception;
|
||||
|
||||
void i2d_set_adresse_esclave(uint8_t _adresse_7bits);
|
||||
void i2c_charger_tampon_envoi(uint8_t* emission, uint16_t nb_envoi, uint16_t nb_reception);
|
||||
enum i2c_resultat_t i2c_transmission(uint8_t _adresse_7bits, uint8_t* emission, uint16_t nb_envoi, uint16_t nb_reception);
|
||||
|
||||
void i2c_maitre_init(void){
|
||||
//stdio_init_all();
|
||||
i2c_init(i2c1, 100 * 1000);
|
||||
|
||||
printf("Initialisation des broches\n");
|
||||
for(int i=0; i++; i<=28){
|
||||
if(gpio_get_function(i) == GPIO_FUNC_I2C){
|
||||
printf("Borche I2C : %d\n", i);
|
||||
gpio_set_function(i, GPIO_FUNC_NULL);
|
||||
}
|
||||
}
|
||||
|
||||
printf("%d et %d en I2C\n", I2C1_SDA_PIN, I2C1_SCL_PIN);
|
||||
gpio_set_function(I2C1_SDA_PIN, GPIO_FUNC_I2C);
|
||||
gpio_set_function(I2C1_SCL_PIN, GPIO_FUNC_I2C);
|
||||
gpio_pull_up(I2C1_SDA_PIN);
|
||||
gpio_pull_up(I2C1_SCL_PIN);
|
||||
|
||||
i2c_statu_i2c1 = I2C_STATU_LIBRE;
|
||||
}
|
||||
|
||||
/// @brief Fonction à appeler régulièrement ou en interruption.
|
||||
/// @param i2c
|
||||
void i2c_gestion(i2c_inst_t *i2c){
|
||||
// on veut gérer l'i2c avec cette fonction.
|
||||
// 2 cas :
|
||||
// - Soit écriture simple (plusieurs octets (W))
|
||||
// - Soit écriture + lecture (Adresse (W), registre (W), données (R))
|
||||
// Pour écrire 1 octet, i2c->hw->data_cmd = xxx, (avec CMD:8 à 0, )
|
||||
// Pour lire 1 octet, i2c->hw->data_cmd = xxx (avec CMD:8 à 1)
|
||||
// Il faut mettre CMD:9 à 1 pour le dernier octet.
|
||||
|
||||
// Envoi des données (ou des demandes de lecture)
|
||||
static uint16_t index_envoi=0, index_reception=0;
|
||||
|
||||
// Acquitement des erreurs, pas 100% fonctionnel ! TODO !
|
||||
if(i2c->hw->tx_abrt_source !=0){
|
||||
// Seule solution trouvée pour réinitialiser l'I2C.
|
||||
char a;
|
||||
i2c_read_blocking(i2c, adresse_7_bits, &a, 1, false);
|
||||
|
||||
I2C_nb_a_envoyer = 0;
|
||||
index_reception = 0;
|
||||
I2C_nb_a_recevoir = 0;
|
||||
statu_emission = TRANSACTION_TERMINEE;
|
||||
statu_reception = TRANSACTION_TERMINEE;
|
||||
i2c_statu_i2c1 = I2C_STATU_LIBRE;
|
||||
}
|
||||
|
||||
while( (index_envoi < I2C_nb_a_envoyer) && (i2c_get_write_available(i2c)) ){
|
||||
bool restart = false;
|
||||
bool last = false;
|
||||
|
||||
if (index_envoi == 0){
|
||||
// Début de l'envoi, assurons nous d'avoir la bonne adresse de l'esclave
|
||||
i2c->hw->enable = 0;
|
||||
i2c->hw->tar = adresse_7_bits;
|
||||
i2c->hw->enable = 1;
|
||||
}else{
|
||||
// Passage de l'écriture à la lecture, on envoie un bit de restart.
|
||||
if( !(I2C_tampon_envoi[index_envoi-1] & I2C_IC_DATA_CMD_CMD_BITS) &&
|
||||
(I2C_tampon_envoi[index_envoi] & I2C_IC_DATA_CMD_CMD_BITS)){
|
||||
restart = true;
|
||||
}
|
||||
}
|
||||
|
||||
if(index_envoi + 1 == I2C_nb_a_envoyer){
|
||||
// Fin de la trame, nous devons envoyer un bit de stop.
|
||||
last = true;
|
||||
}
|
||||
|
||||
i2c->hw->data_cmd =
|
||||
I2C_tampon_envoi[index_envoi] |
|
||||
bool_to_bit(restart) << I2C_IC_DATA_CMD_RESTART_LSB |
|
||||
bool_to_bit(last) << I2C_IC_DATA_CMD_STOP_LSB;
|
||||
|
||||
if(last){
|
||||
statu_emission = TRANSACTION_TERMINEE;
|
||||
index_envoi = 0;
|
||||
I2C_nb_a_envoyer = 0;
|
||||
//printf("I2C emission terminee\n");
|
||||
}else{
|
||||
index_envoi++;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// Réception des données - Lecture des données présentes dans le tampon
|
||||
while( (index_reception < I2C_nb_a_recevoir) && (i2c_get_read_available(i2c)) ){
|
||||
I2C_tampon_reception[index_reception] = (uint8_t) i2c->hw->data_cmd;
|
||||
index_reception++;
|
||||
}
|
||||
if(index_reception == I2C_nb_a_recevoir){
|
||||
statu_reception = TRANSACTION_TERMINEE;
|
||||
index_reception = 0;
|
||||
I2C_nb_a_recevoir = 0;
|
||||
}
|
||||
|
||||
if(statu_reception == TRANSACTION_TERMINEE && statu_emission == TRANSACTION_TERMINEE){
|
||||
i2c_statu_i2c1 = I2C_STATU_LIBRE;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/// @brief Charge le tampon d'émission pour pré-mâcher le travail à la fonction i2c_gestion
|
||||
/// @param emission
|
||||
/// @param nb_envoi
|
||||
/// @param nb_reception
|
||||
void i2c_charger_tampon_envoi(uint8_t* emission, uint16_t nb_envoi, uint16_t nb_reception){
|
||||
// Données à envoyer
|
||||
for(unsigned int index=0; index<nb_envoi; index++){
|
||||
I2C_tampon_envoi[index] = (uint16_t) emission[index];
|
||||
}
|
||||
// Données à lire
|
||||
for(unsigned int index=0; index<nb_reception; index++){
|
||||
I2C_tampon_envoi[nb_envoi + index] = (uint16_t) 0x0100;
|
||||
}
|
||||
}
|
||||
|
||||
/// @brief Stock l'adresse de l'esclave avec lequel communiquer
|
||||
/// @param _adresse_7bits
|
||||
void i2d_set_adresse_esclave(uint8_t _adresse_7bits){
|
||||
adresse_7_bits =_adresse_7bits;
|
||||
}
|
||||
|
||||
/// @brief Initialise la transmission I2, sur l'i2c1. Une transmission se compose de 2 trames I2C, une pour écrire (Adresse + données), une pour lire
|
||||
/// Si nb_reception = 0, alors la trame pour lire ne sera pas envoyée.
|
||||
/// @param emission : données à envoyer
|
||||
/// @param nb_envoi : nombre de données à envoyer
|
||||
/// @param nb_reception : nombre de données à recevoir
|
||||
/// @return I2C_EN_COURS, I2C_SUCCES ou I2C_ECHEC
|
||||
enum i2c_resultat_t i2c_transmission(uint8_t _adresse_7bits, uint8_t* emission, uint16_t nb_envoi, uint16_t nb_reception){
|
||||
static enum m_statu_t{
|
||||
I2C_STATU_INIT,
|
||||
I2C_STATU_EN_COURS,
|
||||
}m_statu = I2C_STATU_INIT;
|
||||
|
||||
switch(m_statu){
|
||||
case I2C_STATU_INIT:
|
||||
// I2C libre ?
|
||||
if(i2c_statu_i2c1 == I2C_STATU_OCCUPE){
|
||||
return I2C_EN_COURS;
|
||||
}
|
||||
// Alors il est à nous !
|
||||
i2c_statu_i2c1 = I2C_STATU_OCCUPE;
|
||||
statu_emission = TRANSACTION_EN_COURS;
|
||||
statu_reception = TRANSACTION_EN_COURS;
|
||||
i2c_error_code = 0;
|
||||
|
||||
i2d_set_adresse_esclave(_adresse_7bits);
|
||||
|
||||
i2c_charger_tampon_envoi(emission, nb_envoi, nb_reception);
|
||||
// Nous devons envoyer aussi une commande pour chaque octet à recevoir.
|
||||
I2C_nb_a_envoyer = nb_envoi + nb_reception;
|
||||
I2C_nb_a_recevoir = nb_reception;
|
||||
|
||||
// On appelle la fonction gestion pour gagner du temps.
|
||||
i2c_gestion(i2c1);
|
||||
m_statu = I2C_STATU_EN_COURS;
|
||||
break;
|
||||
|
||||
case I2C_STATU_EN_COURS:
|
||||
if(i2c_statu_i2c1 == I2C_STATU_LIBRE){
|
||||
m_statu = I2C_STATU_INIT;
|
||||
if(i2c_error_code){
|
||||
return I2C_ECHEC;
|
||||
}else{
|
||||
return I2C_SUCCES;
|
||||
}
|
||||
|
||||
}
|
||||
break;
|
||||
}
|
||||
return I2C_EN_COURS;
|
||||
}
|
||||
|
||||
/// @brief Lit le registre d'un composant se comportant comme une EPROM I2C.
|
||||
/// @return I2C_SUCCES, I2C_EN_COURS ou I2C_ECHEC
|
||||
enum i2c_resultat_t i2c_lire_registre_nb(uint8_t adresse_7_bits, uint8_t registre, uint8_t * reception, uint8_t len){
|
||||
uint8_t emission[1];
|
||||
emission[0] = registre;
|
||||
enum i2c_resultat_t i2c_resultat;
|
||||
i2c_resultat = i2c_transmission(adresse_7_bits, emission, 1, len);
|
||||
if(i2c_resultat == I2C_SUCCES){
|
||||
for(uint32_t i = 0; i < len; i++){
|
||||
reception[i] = I2C_tampon_reception[i];
|
||||
}
|
||||
return I2C_SUCCES;
|
||||
}else if(i2c_resultat == I2C_ECHEC){
|
||||
return I2C_ECHEC;
|
||||
}
|
||||
return I2C_EN_COURS;
|
||||
}
|
||||
|
||||
/// @brief Initialise une transaction I2C.
|
||||
/// Renvoie I2C_SUCCES si l'intégralité du message est chargé en envoi,
|
||||
/// Renvoie I2C_EN_COURS si la fonction doit encore être appelée pour finir d'envoyer le message
|
||||
/// Renvoie I2C_ECHEC en cas d'erreur I2C.
|
||||
enum i2c_resultat_t i2c_ecrire_registre_nb(uint8_t adresse_7_bits, uint8_t registre, uint8_t * _emission, uint8_t len){
|
||||
uint8_t emission[I2C_NB_MAX_TAMPON];
|
||||
emission[0] = registre;
|
||||
for(uint32_t i = 0; i < len; i++){
|
||||
emission[i+1] = _emission[i];
|
||||
}
|
||||
enum i2c_resultat_t i2c_resultat;
|
||||
return i2c_transmission(adresse_7_bits, emission, 1 + len, 0);
|
||||
|
||||
}
|
||||
|
||||
|
||||
/// @brief Pour l'instant bloquant, mais devrait passer en non bloquant bientôt => Non, voir i2c_lire_registre_nb
|
||||
/// @param adresse_7_bits
|
||||
/// @param
|
||||
/// @return I2C_SUCCES (1) ou I2C_ECHEC (2)
|
||||
int i2c_lire_registre(char adresse_7_bits, char registre, unsigned char * reception, char len){
|
||||
int statu;
|
||||
char emission[1];
|
||||
|
||||
emission[0] = registre;
|
||||
statu = i2c_write_blocking (i2c1, adresse_7_bits, emission, 1, 0);
|
||||
if(statu == PICO_ERROR_GENERIC){
|
||||
printf("I2C - Envoi registre Echec\n");
|
||||
return I2C_ECHEC;
|
||||
}
|
||||
|
||||
statu = i2c_read_blocking (i2c1, adresse_7_bits, reception, len, 0);
|
||||
if(statu == PICO_ERROR_GENERIC){
|
||||
printf("I2C - Lecture registre Echec\n");
|
||||
return I2C_ECHEC;
|
||||
}
|
||||
|
||||
return I2C_SUCCES;
|
||||
}
|
||||
|
||||
int i2c_ecrire_registre(char adresse_7_bits, char registre, char valeur_registre){
|
||||
int statu;
|
||||
char emission[2];
|
||||
|
||||
emission[0] = registre;
|
||||
emission[1] = valeur_registre;
|
||||
statu = i2c_write_blocking (i2c1, adresse_7_bits, emission, 2, 0);
|
||||
if(statu == PICO_ERROR_GENERIC){
|
||||
printf("Erreur ecrire registre\n");
|
||||
return I2C_ECHEC;
|
||||
}
|
||||
|
||||
printf("i2c Registre %x, valeur %x\n", registre, valeur_registre);
|
||||
|
||||
return I2C_SUCCES;
|
||||
}
|
||||
|
|
@ -0,0 +1,15 @@
|
|||
#include "pico/stdlib.h"
|
||||
#include "hardware/i2c.h"
|
||||
|
||||
enum i2c_resultat_t {
|
||||
I2C_EN_COURS,
|
||||
I2C_SUCCES,
|
||||
I2C_ECHEC
|
||||
};
|
||||
|
||||
void i2c_maitre_init(void);
|
||||
void i2c_gestion(i2c_inst_t *i2c);
|
||||
enum i2c_resultat_t i2c_lire_registre_nb(uint8_t adresse_7_bits, uint8_t registre, uint8_t * reception, uint8_t len);
|
||||
enum i2c_resultat_t i2c_ecrire_registre_nb(uint8_t adresse_7_bits, uint8_t registre, uint8_t * _emission, uint8_t len);
|
||||
int i2c_ecrire_registre(char adresse_7_bits, char registre, char valeur_registre);
|
||||
int i2c_lire_registre(char adresse_7_bits, char registre, unsigned char * reception, char len);
|
||||
|
|
@ -0,0 +1,108 @@
|
|||
/*
|
||||
* Copyright (c) 2021 Valentin Milea <valentin.milea@gmail.com>
|
||||
*
|
||||
* SPDX-License-Identifier: MIT
|
||||
*/
|
||||
|
||||
#include "i2c_slave.h"
|
||||
#include "hardware/irq.h"
|
||||
|
||||
typedef struct i2c_slave_t
|
||||
{
|
||||
i2c_inst_t *i2c;
|
||||
i2c_slave_handler_t handler;
|
||||
bool transfer_in_progress;
|
||||
} i2c_slave_t;
|
||||
|
||||
static i2c_slave_t i2c_slaves[2];
|
||||
|
||||
static inline void finish_transfer(i2c_slave_t *slave) {
|
||||
if (slave->transfer_in_progress) {
|
||||
slave->handler(slave->i2c, I2C_SLAVE_FINISH);
|
||||
slave->transfer_in_progress = false;
|
||||
}
|
||||
}
|
||||
|
||||
static void __not_in_flash_func(i2c_slave_irq_handler)(i2c_slave_t *slave) {
|
||||
i2c_inst_t *i2c = slave->i2c;
|
||||
i2c_hw_t *hw = i2c_get_hw(i2c);
|
||||
|
||||
uint32_t intr_stat = hw->intr_stat;
|
||||
if (intr_stat == 0) {
|
||||
return;
|
||||
}
|
||||
if (intr_stat & I2C_IC_INTR_STAT_R_TX_ABRT_BITS) {
|
||||
hw->clr_tx_abrt;
|
||||
finish_transfer(slave);
|
||||
}
|
||||
if (intr_stat & I2C_IC_INTR_STAT_R_START_DET_BITS) {
|
||||
hw->clr_start_det;
|
||||
finish_transfer(slave);
|
||||
}
|
||||
if (intr_stat & I2C_IC_INTR_STAT_R_STOP_DET_BITS) {
|
||||
hw->clr_stop_det;
|
||||
finish_transfer(slave);
|
||||
}
|
||||
if (intr_stat & I2C_IC_INTR_STAT_R_RX_FULL_BITS) {
|
||||
slave->transfer_in_progress = true;
|
||||
slave->handler(i2c, I2C_SLAVE_RECEIVE);
|
||||
}
|
||||
if (intr_stat & I2C_IC_INTR_STAT_R_RD_REQ_BITS) {
|
||||
hw->clr_rd_req;
|
||||
slave->transfer_in_progress = true;
|
||||
slave->handler(i2c, I2C_SLAVE_REQUEST);
|
||||
}
|
||||
}
|
||||
|
||||
static void __not_in_flash_func(i2c0_slave_irq_handler)() {
|
||||
i2c_slave_irq_handler(&i2c_slaves[0]);
|
||||
}
|
||||
|
||||
static void __not_in_flash_func(i2c1_slave_irq_handler)() {
|
||||
i2c_slave_irq_handler(&i2c_slaves[1]);
|
||||
}
|
||||
|
||||
void i2c_slave_init(i2c_inst_t *i2c, uint8_t address, i2c_slave_handler_t handler) {
|
||||
assert(i2c == i2c0 || i2c == i2c1);
|
||||
assert(handler != NULL);
|
||||
|
||||
uint i2c_index = i2c_hw_index(i2c);
|
||||
i2c_slave_t *slave = &i2c_slaves[i2c_index];
|
||||
slave->i2c = i2c;
|
||||
slave->handler = handler;
|
||||
|
||||
// Note: The I2C slave does clock stretching implicitly after a RD_REQ, while the Tx FIFO is empty.
|
||||
// There is also an option to enable clock stretching while the Rx FIFO is full, but we leave it
|
||||
// disabled since the Rx FIFO should never fill up (unless slave->handler() is way too slow).
|
||||
i2c_set_slave_mode(i2c, true, address);
|
||||
|
||||
i2c_hw_t *hw = i2c_get_hw(i2c);
|
||||
// unmask necessary interrupts
|
||||
hw->intr_mask = I2C_IC_INTR_MASK_M_RX_FULL_BITS | I2C_IC_INTR_MASK_M_RD_REQ_BITS | I2C_IC_RAW_INTR_STAT_TX_ABRT_BITS | I2C_IC_INTR_MASK_M_STOP_DET_BITS | I2C_IC_INTR_MASK_M_START_DET_BITS;
|
||||
|
||||
// enable interrupt for current core
|
||||
uint num = I2C0_IRQ + i2c_index;
|
||||
irq_set_exclusive_handler(num, i2c_index == 0 ? i2c0_slave_irq_handler : i2c1_slave_irq_handler);
|
||||
irq_set_enabled(num, true);
|
||||
}
|
||||
|
||||
void i2c_slave_deinit(i2c_inst_t *i2c) {
|
||||
assert(i2c == i2c0 || i2c == i2c1);
|
||||
|
||||
uint i2c_index = i2c_hw_index(i2c);
|
||||
i2c_slave_t *slave = &i2c_slaves[i2c_index];
|
||||
assert(slave->i2c == i2c); // should be called after i2c_slave_init()
|
||||
|
||||
slave->i2c = NULL;
|
||||
slave->handler = NULL;
|
||||
slave->transfer_in_progress = false;
|
||||
|
||||
uint num = I2C0_IRQ + i2c_index;
|
||||
irq_set_enabled(num, false);
|
||||
irq_remove_handler(num, i2c_index == 0 ? i2c0_slave_irq_handler : i2c1_slave_irq_handler);
|
||||
|
||||
i2c_hw_t *hw = i2c_get_hw(i2c);
|
||||
hw->intr_mask = I2C_IC_INTR_MASK_RESET;
|
||||
|
||||
i2c_set_slave_mode(i2c, false, 0);
|
||||
}
|
||||
|
|
@ -0,0 +1,66 @@
|
|||
/*
|
||||
* Copyright (c) 2021 Valentin Milea <valentin.milea@gmail.com>
|
||||
*
|
||||
* SPDX-License-Identifier: MIT
|
||||
*/
|
||||
|
||||
#ifndef _I2C_SLAVE_H_
|
||||
#define _I2C_SLAVE_H_
|
||||
|
||||
#include <hardware/i2c.h>
|
||||
#include <pico/stdlib.h>
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/** \file i2c_slave.h
|
||||
*
|
||||
* \brief I2C slave setup.
|
||||
*/
|
||||
|
||||
/**
|
||||
* \brief I2C slave event types.
|
||||
*/
|
||||
typedef enum i2c_slave_event_t
|
||||
{
|
||||
I2C_SLAVE_RECEIVE, /**< Data from master is available for reading. Slave must read from Rx FIFO. */
|
||||
I2C_SLAVE_REQUEST, /**< Master is requesting data. Slave must write into Tx FIFO. */
|
||||
I2C_SLAVE_FINISH, /**< Master has sent a Stop or Restart signal. Slave may prepare for the next transfer. */
|
||||
} i2c_slave_event_t;
|
||||
|
||||
/**
|
||||
* \brief I2C slave event handler
|
||||
*
|
||||
* The event handler will run from the I2C ISR, so it should return quickly (under 25 us at 400 kb/s).
|
||||
* Avoid blocking inside the handler and split large data transfers across multiple calls for best results.
|
||||
* When sending data to master, up to `i2c_get_write_available()` bytes can be written without blocking.
|
||||
* When receiving data from master, up to `i2c_get_read_available()` bytes can be read without blocking.
|
||||
*
|
||||
* \param i2c Slave I2C instance.
|
||||
* \param event Event type.
|
||||
*/
|
||||
typedef void (*i2c_slave_handler_t)(i2c_inst_t *i2c, i2c_slave_event_t event);
|
||||
|
||||
/**
|
||||
* \brief Configure I2C instance for slave mode.
|
||||
*
|
||||
* \param i2c I2C instance.
|
||||
* \param address 7-bit slave address.
|
||||
* \param handler Called on events from I2C master. It will run from the I2C ISR, on the CPU core
|
||||
* where the slave was initialized.
|
||||
*/
|
||||
void i2c_slave_init(i2c_inst_t *i2c, uint8_t address, i2c_slave_handler_t handler);
|
||||
|
||||
/**
|
||||
* \brief Restore I2C instance to master mode.
|
||||
*
|
||||
* \param i2c I2C instance.
|
||||
*/
|
||||
void i2c_slave_deinit(i2c_inst_t *i2c);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // _I2C_SLAVE_H_
|
||||
|
|
@ -0,0 +1,172 @@
|
|||
/*****
|
||||
* Copyright (c) 2023 - Poivron Robotique
|
||||
*
|
||||
* SPDX-License-Identifier: BSD-3-Clause
|
||||
*/
|
||||
#include "communication.h"
|
||||
#include "pico/stdlib.h"
|
||||
#include "hardware/pwm.h"
|
||||
#include <stdio.h>
|
||||
#include "hardware/adc.h"
|
||||
#define LED_VERTE 25
|
||||
#define PIN_VITESSE_M1 2
|
||||
#define PIN_SENS_A_M1 0
|
||||
#define PIN_SENS_B_M1 1
|
||||
|
||||
int result0, result1;
|
||||
int joystic_clicker;
|
||||
int pince;
|
||||
int ascenceur;
|
||||
|
||||
int M1_INITIALISE()
|
||||
{
|
||||
gpio_init(PIN_VITESSE_M1);
|
||||
gpio_init(PIN_SENS_A_M1);
|
||||
gpio_init(PIN_SENS_B_M1);
|
||||
|
||||
gpio_set_dir(PIN_VITESSE_M1, GPIO_OUT);
|
||||
gpio_set_dir(PIN_SENS_A_M1, GPIO_OUT);
|
||||
gpio_set_dir(PIN_SENS_B_M1, GPIO_OUT);
|
||||
|
||||
gpio_set_function(PIN_VITESSE_M1, GPIO_FUNC_PWM);
|
||||
pwm_set_wrap(PIN_VITESSE_M1, 100);
|
||||
pwm_set_chan_level(PIN_VITESSE_M1, PWM_CHAN_B, 50);
|
||||
|
||||
}
|
||||
|
||||
int M1_AVANCE()
|
||||
{
|
||||
|
||||
gpio_put(PIN_VITESSE_M1, 1);
|
||||
gpio_put(PIN_SENS_A_M1, 1);
|
||||
gpio_put(PIN_SENS_B_M1, 0);
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
int M1_RECULE()
|
||||
{
|
||||
|
||||
gpio_put(PIN_VITESSE_M1, 1);
|
||||
gpio_put(PIN_SENS_A_M1, 0);
|
||||
gpio_put(PIN_SENS_B_M1, 1);
|
||||
|
||||
|
||||
|
||||
}
|
||||
int Adc_Init()
|
||||
{
|
||||
adc_init();
|
||||
adc_gpio_init(26);
|
||||
adc_gpio_init(27);
|
||||
}
|
||||
int AdcRead0()
|
||||
{
|
||||
adc_select_input(0);
|
||||
uint16_t resultadc = adc_read();
|
||||
return resultadc;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
int AdcRead1()
|
||||
{
|
||||
adc_select_input(1);
|
||||
uint16_t resultadc = adc_read();
|
||||
return resultadc;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
void main()
|
||||
{
|
||||
char message [256];
|
||||
stdio_init_all();
|
||||
|
||||
// Communication
|
||||
communication_init();
|
||||
|
||||
// Pour envoyer un message
|
||||
// communication_envoyer_message(message, 254);
|
||||
|
||||
|
||||
// CLignottement LED
|
||||
gpio_set_function(LED_VERTE, GPIO_FUNC_PWM);
|
||||
pwm_set_wrap(4, 100);
|
||||
pwm_set_chan_level(4, PWM_CHAN_B, 25);
|
||||
pwm_set_enabled(4, true);
|
||||
|
||||
// ADC
|
||||
adc_init();
|
||||
adc_gpio_init(26);
|
||||
adc_gpio_init(27);
|
||||
|
||||
while (1)
|
||||
{
|
||||
// Voie X
|
||||
result1 = AdcRead1();
|
||||
|
||||
message[0] = result1/16;
|
||||
|
||||
// Voie Y
|
||||
result0 = AdcRead0();
|
||||
|
||||
message[1] = result0/16;
|
||||
|
||||
//clic sur le joystic
|
||||
gpio_init(1);
|
||||
gpio_pull_up(1);
|
||||
gpio_set_dir(1, GPIO_IN);
|
||||
joystic_clicker = gpio_get(1);
|
||||
if (!joystic_clicker)
|
||||
{
|
||||
message[2] = result1/16;
|
||||
message[1] = 128;
|
||||
message[0] = 128;
|
||||
}
|
||||
else
|
||||
{
|
||||
message[2] = 128;
|
||||
}
|
||||
|
||||
|
||||
|
||||
//pince
|
||||
gpio_init(2);
|
||||
gpio_set_dir(2, GPIO_IN);
|
||||
gpio_pull_up(2);
|
||||
pince = gpio_get(2);
|
||||
message [3] = pince;
|
||||
|
||||
gpio_init(6);
|
||||
gpio_set_dir(6, GPIO_IN);
|
||||
gpio_pull_up(6);
|
||||
ascenceur = gpio_get(6);
|
||||
message[4] = ascenceur;
|
||||
|
||||
|
||||
printf(">x:%d\n", message[0]);
|
||||
printf(">Y:%d\n", message[1]);
|
||||
printf(">Rz:%d\n", message[2]);
|
||||
printf(">pince:%d\n", message[3]);
|
||||
printf(">ascenceur:%d\n", message[4]);
|
||||
sleep_ms(100);
|
||||
communication_envoyer_message(message, 254);
|
||||
}
|
||||
M1_INITIALISE();
|
||||
while(1)
|
||||
{
|
||||
M1_AVANCE();
|
||||
sleep_ms(1000);
|
||||
M1_RECULE();
|
||||
sleep_ms(1000);
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,62 @@
|
|||
# This is a copy of <PICO_SDK_PATH>/external/pico_sdk_import.cmake
|
||||
|
||||
# This can be dropped into an external project to help locate this SDK
|
||||
# It should be include()ed prior to project()
|
||||
|
||||
if (DEFINED ENV{PICO_SDK_PATH} AND (NOT PICO_SDK_PATH))
|
||||
set(PICO_SDK_PATH $ENV{PICO_SDK_PATH})
|
||||
message("Using PICO_SDK_PATH from environment ('${PICO_SDK_PATH}')")
|
||||
endif ()
|
||||
|
||||
if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT} AND (NOT PICO_SDK_FETCH_FROM_GIT))
|
||||
set(PICO_SDK_FETCH_FROM_GIT $ENV{PICO_SDK_FETCH_FROM_GIT})
|
||||
message("Using PICO_SDK_FETCH_FROM_GIT from environment ('${PICO_SDK_FETCH_FROM_GIT}')")
|
||||
endif ()
|
||||
|
||||
if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT_PATH} AND (NOT PICO_SDK_FETCH_FROM_GIT_PATH))
|
||||
set(PICO_SDK_FETCH_FROM_GIT_PATH $ENV{PICO_SDK_FETCH_FROM_GIT_PATH})
|
||||
message("Using PICO_SDK_FETCH_FROM_GIT_PATH from environment ('${PICO_SDK_FETCH_FROM_GIT_PATH}')")
|
||||
endif ()
|
||||
|
||||
set(PICO_SDK_PATH "${PICO_SDK_PATH}" CACHE PATH "Path to the Raspberry Pi Pico SDK")
|
||||
set(PICO_SDK_FETCH_FROM_GIT "${PICO_SDK_FETCH_FROM_GIT}" CACHE BOOL "Set to ON to fetch copy of SDK from git if not otherwise locatable")
|
||||
set(PICO_SDK_FETCH_FROM_GIT_PATH "${PICO_SDK_FETCH_FROM_GIT_PATH}" CACHE FILEPATH "location to download SDK")
|
||||
|
||||
if (NOT PICO_SDK_PATH)
|
||||
if (PICO_SDK_FETCH_FROM_GIT)
|
||||
include(FetchContent)
|
||||
set(FETCHCONTENT_BASE_DIR_SAVE ${FETCHCONTENT_BASE_DIR})
|
||||
if (PICO_SDK_FETCH_FROM_GIT_PATH)
|
||||
get_filename_component(FETCHCONTENT_BASE_DIR "${PICO_SDK_FETCH_FROM_GIT_PATH}" REALPATH BASE_DIR "${CMAKE_SOURCE_DIR}")
|
||||
endif ()
|
||||
FetchContent_Declare(
|
||||
pico_sdk
|
||||
GIT_REPOSITORY https://github.com/raspberrypi/pico-sdk
|
||||
GIT_TAG master
|
||||
)
|
||||
if (NOT pico_sdk)
|
||||
message("Downloading Raspberry Pi Pico SDK")
|
||||
FetchContent_Populate(pico_sdk)
|
||||
set(PICO_SDK_PATH ${pico_sdk_SOURCE_DIR})
|
||||
endif ()
|
||||
set(FETCHCONTENT_BASE_DIR ${FETCHCONTENT_BASE_DIR_SAVE})
|
||||
else ()
|
||||
message(FATAL_ERROR
|
||||
"SDK location was not specified. Please set PICO_SDK_PATH or set PICO_SDK_FETCH_FROM_GIT to on to fetch from git."
|
||||
)
|
||||
endif ()
|
||||
endif ()
|
||||
|
||||
get_filename_component(PICO_SDK_PATH "${PICO_SDK_PATH}" REALPATH BASE_DIR "${CMAKE_BINARY_DIR}")
|
||||
if (NOT EXISTS ${PICO_SDK_PATH})
|
||||
message(FATAL_ERROR "Directory '${PICO_SDK_PATH}' not found")
|
||||
endif ()
|
||||
|
||||
set(PICO_SDK_INIT_CMAKE_FILE ${PICO_SDK_PATH}/pico_sdk_init.cmake)
|
||||
if (NOT EXISTS ${PICO_SDK_INIT_CMAKE_FILE})
|
||||
message(FATAL_ERROR "Directory '${PICO_SDK_PATH}' does not appear to contain the Raspberry Pi Pico SDK")
|
||||
endif ()
|
||||
|
||||
set(PICO_SDK_PATH ${PICO_SDK_PATH} CACHE PATH "Path to the Raspberry Pi Pico SDK" FORCE)
|
||||
|
||||
include(${PICO_SDK_INIT_CMAKE_FILE})
|
||||
|
|
@ -0,0 +1,55 @@
|
|||
#ifndef MOTORS_H
|
||||
#define MOTORS_H
|
||||
|
||||
// Motors
|
||||
typedef enum motors_enum_t {
|
||||
MOTOR1,
|
||||
MOTOR2,
|
||||
MOTOR3,
|
||||
MOTOR4,
|
||||
|
||||
NB_MOTORS
|
||||
} motors_enum_t;
|
||||
|
||||
typedef struct motor_def_t {
|
||||
uint pwm_pin;
|
||||
uint dir1_pin;
|
||||
uint dir2_pin;
|
||||
uint8_t buffer_address;
|
||||
} motor_def_t;
|
||||
|
||||
extern const motor_def_t MOTORS_DEFS[];
|
||||
|
||||
// Servo Motors
|
||||
typedef enum {
|
||||
SERVO_MOTOR1,
|
||||
SERVO_MOTOR2,
|
||||
SERVO_MOTOR3,
|
||||
SERVO_MOTOR4,
|
||||
|
||||
NB_SERVO_MOTORS
|
||||
} servo_motors_enum_t;
|
||||
|
||||
typedef struct {
|
||||
uint pwm_pin;
|
||||
uint open_pos;
|
||||
uint close_pos;
|
||||
uint8_t buffer_address;
|
||||
} servo_motor_def_t;
|
||||
|
||||
extern const servo_motor_def_t SERVO_MOTORS_DEFS[];
|
||||
|
||||
// Init all motors defined in the MOTORS_DEF array
|
||||
void init_motors(void);
|
||||
// Init all servo motors defined in the SERVO_MOTORS_DEF array
|
||||
void init_servo_motors(void);
|
||||
// Set [motor] to 0
|
||||
void motor_zero(motors_enum_t motor);
|
||||
// Set [motor] in motor_enum_t at [value] between -127 and 128 (for this config)
|
||||
void motor_set(motors_enum_t motor, int value);
|
||||
// Set servo motor to its open pos
|
||||
void servo_motor_zero(servo_motors_enum_t servo_motor);
|
||||
// Set servo to its close pos if [close] else open pos
|
||||
void servo_motor_set(servo_motors_enum_t servo_motor, bool close);
|
||||
|
||||
#endif // MOTORS_H
|
||||
|
|
@ -0,0 +1,18 @@
|
|||
#ifndef ROBOT_H
|
||||
#define ROBOT_H
|
||||
|
||||
typedef struct robot_t {
|
||||
bool is_running;
|
||||
double delta_time_ms;
|
||||
} robot_t;
|
||||
|
||||
extern robot_t robot;
|
||||
|
||||
// Init all robot's components
|
||||
void robot_init(void);
|
||||
// Handle inputs and outputs
|
||||
void robot_handle_inputs_outputs(void);
|
||||
// Deinit all robot's components
|
||||
void robot_deinit(void);
|
||||
|
||||
#endif // ROBOT_H
|
||||
|
|
@ -0,0 +1,23 @@
|
|||
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *\
|
||||
* Code du RPI Pico principale gérant les differentes entrées-sorties. *
|
||||
* Ce Pico est un maitre pilotant le gyroscope, l'internet et le motion controller.*
|
||||
\* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
|
||||
|
||||
#include <stdbool.h>
|
||||
#include "include/robot.h"
|
||||
|
||||
robot_t robot;
|
||||
|
||||
int main(void)
|
||||
{
|
||||
robot_init();
|
||||
|
||||
while(robot.is_running)
|
||||
{
|
||||
robot_handle_inputs_outputs();
|
||||
}
|
||||
|
||||
robot_deinit();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -0,0 +1,119 @@
|
|||
#include <pico/stdlib.h>
|
||||
#include <hardware/pwm.h>
|
||||
|
||||
#include "include/motors.h"
|
||||
|
||||
const motor_def_t MOTORS_DEFS[] = {
|
||||
{0, 4, 5, 0x00},
|
||||
{1, 6, 7, 0x01},
|
||||
{2, 8, 9, 0x02},
|
||||
{3, 10, 11, 0x03},
|
||||
};
|
||||
|
||||
const servo_motor_def_t SERVO_MOTORS_DEFS[] = {
|
||||
{12, 0, 25000, 0x04},
|
||||
{13, 0, 25000, 0x05},
|
||||
{14, 0, 25000, 0x06},
|
||||
{15, 0, 25000, 0x07},
|
||||
};
|
||||
|
||||
// Init all motors defined in the MOTORS_DEF array
|
||||
void init_motors(void)
|
||||
{
|
||||
for(motors_enum_t actual_motor = MOTOR1; actual_motor < NB_MOTORS; actual_motor++)
|
||||
{
|
||||
const motor_def_t *motor_def = &MOTORS_DEFS[actual_motor];
|
||||
|
||||
// Init PWM
|
||||
uint slice_num = pwm_gpio_to_slice_num(motor_def->pwm_pin);
|
||||
|
||||
gpio_set_function(motor_def->pwm_pin, GPIO_FUNC_PWM);
|
||||
pwm_set_wrap(slice_num, 128);
|
||||
pwm_set_enabled(slice_num, true);
|
||||
|
||||
// Init dir pins
|
||||
gpio_init(motor_def->dir1_pin);
|
||||
gpio_set_dir(motor_def->dir1_pin, GPIO_OUT);
|
||||
|
||||
gpio_init(motor_def->dir2_pin);
|
||||
gpio_set_dir(motor_def->dir2_pin, GPIO_OUT);
|
||||
|
||||
motor_zero(actual_motor);
|
||||
}
|
||||
}
|
||||
|
||||
// Init all servo motors defined in the SERVO_MOTORS_DEF array
|
||||
void init_servo_motors(void)
|
||||
{
|
||||
for(servo_motors_enum_t actual_servo_motor = SERVO_MOTOR1; actual_servo_motor < NB_SERVO_MOTORS; actual_servo_motor++)
|
||||
{
|
||||
const servo_motor_def_t *servo_motor_def = &SERVO_MOTORS_DEFS[actual_servo_motor];
|
||||
|
||||
// Init PWM //
|
||||
uint slice_num = pwm_gpio_to_slice_num(servo_motor_def->pwm_pin);
|
||||
|
||||
gpio_set_function(servo_motor_def->pwm_pin, GPIO_FUNC_PWM);
|
||||
pwm_set_wrap(slice_num, 25000);
|
||||
pwm_set_clkdiv(slice_num, 100);
|
||||
pwm_set_enabled(slice_num, true);
|
||||
|
||||
servo_motor_zero(actual_servo_motor);
|
||||
}
|
||||
}
|
||||
|
||||
// Set [motor] to 0
|
||||
void motor_zero(motors_enum_t motor)
|
||||
{
|
||||
const motor_def_t *motor_def = &MOTORS_DEFS[motor];
|
||||
|
||||
// Set PWM to zero
|
||||
pwm_set_gpio_level(motor_def->pwm_pin, 0);
|
||||
|
||||
// Set dir pins to false
|
||||
gpio_put(motor_def->dir1_pin, false);
|
||||
gpio_put(motor_def->dir2_pin, false);
|
||||
}
|
||||
|
||||
// Set [motor] in motor_enum_t at [value] between -128 and 127 (for this config)
|
||||
void motor_set(motors_enum_t motor, int value)
|
||||
{
|
||||
const motor_def_t *motor_def = &MOTORS_DEFS[motor];
|
||||
|
||||
if(value < 0)
|
||||
{
|
||||
gpio_put(motor_def->dir1_pin, true);
|
||||
gpio_put(motor_def->dir2_pin, false);
|
||||
|
||||
value = -value;
|
||||
}
|
||||
else if(value > 0)
|
||||
{
|
||||
gpio_put(motor_def->dir1_pin, false);
|
||||
gpio_put(motor_def->dir2_pin, true);
|
||||
}
|
||||
else
|
||||
{
|
||||
gpio_put(motor_def->dir1_pin, false);
|
||||
gpio_put(motor_def->dir2_pin, false);
|
||||
}
|
||||
|
||||
pwm_set_gpio_level(motor_def->pwm_pin, (uint16_t)value);
|
||||
}
|
||||
|
||||
// Set servo motor to its open pos
|
||||
void servo_motor_zero(servo_motors_enum_t servo_motor)
|
||||
{
|
||||
const servo_motor_def_t *servo_motor_def = &SERVO_MOTORS_DEFS[servo_motor];
|
||||
|
||||
// Set PWM to zero //
|
||||
pwm_set_gpio_level(servo_motor_def->pwm_pin, servo_motor_def->open_pos);
|
||||
}
|
||||
|
||||
// Set servo to its close pos if [close] else open pos
|
||||
void servo_motor_set(servo_motors_enum_t servo_motor, bool close)
|
||||
{
|
||||
const servo_motor_def_t *servo_motor_def = &SERVO_MOTORS_DEFS[servo_motor];
|
||||
|
||||
// Set PWM to zero //
|
||||
pwm_set_gpio_level(servo_motor_def->pwm_pin, close ? servo_motor_def->close_pos : servo_motor_def->open_pos);
|
||||
}
|
||||
|
|
@ -0,0 +1,26 @@
|
|||
#include <pico/stdlib.h>
|
||||
#include "include/motors.h"
|
||||
#include "include/i2c_slave.h"
|
||||
|
||||
#include "include/robot.h"
|
||||
|
||||
void robot_init(void)
|
||||
{
|
||||
stdio_init_all();
|
||||
|
||||
init_motors();
|
||||
init_servo_motors();
|
||||
i2c_slave_init();
|
||||
|
||||
robot.is_running = true;
|
||||
}
|
||||
|
||||
void robot_handle_inputs_outputs(void)
|
||||
{
|
||||
update_motors_from_buffer();
|
||||
}
|
||||
|
||||
void robot_deinit(void)
|
||||
{
|
||||
i2c_slave_deinit();
|
||||
}
|
||||
Loading…
Reference in New Issue