135 lines
2.4 KiB
C
135 lines
2.4 KiB
C
/*****
|
|
* Copyright (c) 2023 - Poivron Robotique
|
|
*
|
|
* SPDX-License-Identifier: BSD-3-Clause
|
|
*/
|
|
#include "pico/stdlib.h"
|
|
#include "hardware/pwm.h"
|
|
#include <stdio.h>
|
|
#include "QEI.h"
|
|
|
|
#define LED1PIN 20
|
|
#define M1_SENS1 7
|
|
#define M1_SENS2 13
|
|
#define M1_VITESSE 27 //5B
|
|
#define M2_SENS1 10
|
|
#define M2_SENS2 5
|
|
#define M2_VITESSE 9 //4B
|
|
|
|
void Moteur_init(void);
|
|
void M1_forward(int speed);
|
|
void M1_backward(int speed);
|
|
void M1_speed(int speed);
|
|
void M2_forward(int speed);
|
|
void M2_backward(int speed);
|
|
void M2_speed(int speed);
|
|
|
|
void main(void)
|
|
{
|
|
int ledpower = 500;
|
|
|
|
stdio_init_all();
|
|
QEI_init();
|
|
Moteur_init();
|
|
|
|
gpio_init(LED1PIN);
|
|
gpio_set_dir(LED1PIN, GPIO_OUT );
|
|
gpio_put(LED1PIN, 1);
|
|
|
|
while(1){
|
|
QEI_update();
|
|
|
|
printf(">c1:%d\n>c2:%d\n", QEI_get(QEI_A_NAME), QEI_get(QEI_B_NAME) );
|
|
|
|
M1_speed(1000);
|
|
M2_speed(1000);
|
|
|
|
sleep_ms(100);
|
|
}
|
|
}
|
|
|
|
void Moteur_init(void){
|
|
|
|
gpio_init(M1_SENS1);
|
|
gpio_init(M1_SENS2);
|
|
gpio_init(M2_SENS1);
|
|
gpio_init(M2_SENS2);
|
|
gpio_set_dir(M1_SENS1, GPIO_OUT);
|
|
gpio_set_dir(M1_SENS2, GPIO_OUT);
|
|
gpio_set_dir(M2_SENS1, GPIO_OUT);
|
|
gpio_set_dir(M2_SENS2, GPIO_OUT);
|
|
gpio_put(M1_SENS1, 0);
|
|
gpio_put(M1_SENS2, 0);
|
|
gpio_put(M2_SENS1, 0);
|
|
gpio_put(M2_SENS2, 0);
|
|
|
|
gpio_set_function(M1_VITESSE, GPIO_FUNC_PWM);
|
|
gpio_set_function(M2_VITESSE, GPIO_FUNC_PWM);
|
|
pwm_set_wrap(5, 1000);
|
|
pwm_set_wrap(4, 1000);
|
|
pwm_set_chan_level(5, PWM_CHAN_B, 0);
|
|
pwm_set_chan_level(4, PWM_CHAN_B, 0);
|
|
pwm_set_enabled(5, true);
|
|
pwm_set_enabled(4, true);
|
|
|
|
}
|
|
|
|
void M1_forward(int speed)
|
|
{
|
|
pwm_set_chan_level(5, PWM_CHAN_B, speed);
|
|
gpio_put(M1_SENS1, 1);
|
|
gpio_put(M1_SENS2, 0);
|
|
}
|
|
|
|
// Set motor 1 speed backward
|
|
void M1_backward(int speed)
|
|
{
|
|
pwm_set_chan_level(5, PWM_CHAN_B, speed);
|
|
gpio_put(M1_SENS1, 0);
|
|
gpio_put(M1_SENS2, 1);
|
|
}
|
|
|
|
// Set motor 1 speed and direction (negative value : backward / positive value : forward)
|
|
void M1_speed(int speed)
|
|
{
|
|
if(speed < 0)
|
|
{
|
|
speed = -speed;
|
|
M1_backward(speed);
|
|
}
|
|
else
|
|
{
|
|
M1_forward(speed);
|
|
}
|
|
}
|
|
|
|
// Set motor 2 speed forward
|
|
void M2_forward(int speed)
|
|
{
|
|
pwm_set_chan_level(4, PWM_CHAN_B, speed);
|
|
gpio_put(M2_SENS1, 0);
|
|
gpio_put(M2_SENS2, 1);
|
|
}
|
|
|
|
// Set motor 2 speed backward
|
|
void M2_backward(int speed)
|
|
{
|
|
pwm_set_chan_level(4, PWM_CHAN_B, speed);
|
|
gpio_put(M2_SENS1, 1);
|
|
gpio_put(M2_SENS2, 0);
|
|
}
|
|
|
|
// Set motor 2 speed and direction (negative value : backward / positive value : forward)
|
|
void M2_speed(int speed)
|
|
{
|
|
if(speed < 0)
|
|
{
|
|
speed = -speed;
|
|
M2_backward(speed);
|
|
}
|
|
else
|
|
{
|
|
M2_forward(speed);
|
|
}
|
|
}
|