101 lines
2.1 KiB
C
101 lines
2.1 KiB
C
#include "pico/stdlib.h"
|
|
#include "hardware/adc.h"
|
|
#include "communication.h"
|
|
#include "Servomoteur.h"
|
|
#include <stdio.h>
|
|
#include "moteur.h"
|
|
|
|
// Juste pour information - les broches sont re-définies dans i2c_slave
|
|
#define I2C0_SDA_PIN 16
|
|
#define I2C0_SCL_PIN 17
|
|
|
|
// Juste pour information - les broches sont re-définies dans i2c_master
|
|
#define I2C1_SDA_PIN 18
|
|
#define I2C1_SCL_PIN 19
|
|
|
|
// Allumage de la led verte
|
|
#define PIN_LED_VERTE 25
|
|
|
|
void main(void)
|
|
{
|
|
char message[256], reception[256];
|
|
|
|
// Init "all"
|
|
stdio_init_all();
|
|
Init_motion_motor();
|
|
Servomoteur_init();
|
|
communication_init();
|
|
|
|
gpio_init(PIN_LED_VERTE);
|
|
gpio_set_function(PIN_LED_VERTE, GPIO_FUNC_PWM);
|
|
pwm_set_clkdiv(4, 100);
|
|
pwm_set_wrap(4, 100);
|
|
pwm_set_enabled(4, true);
|
|
pwm_set_chan_level(4, PWM_CHAN_B, 100);
|
|
|
|
int vitesse_angle = 0;
|
|
int vitesse_m1 = 0;
|
|
int vitesse_m2 = 0;
|
|
int vitesse_m3 = 0;
|
|
int vitesse_m4 = 0;
|
|
|
|
while(1){
|
|
|
|
if (!(communication_read_message(reception) == I2C_ECHEC))
|
|
{
|
|
vitesse_m1 = reception[0] - 128;
|
|
vitesse_m2 = reception[0] - 128;
|
|
vitesse_m3 = reception[1] - 128;
|
|
vitesse_m4 = reception[1] - 128;
|
|
|
|
vitesse_angle = reception[2] - 128;
|
|
|
|
switch(reception[3]){
|
|
case 0: Servo_pince_lache(); break;
|
|
case 1: Servo_pince_tient(); break;
|
|
}
|
|
|
|
switch(reception[4]){
|
|
case 0: Servo_plie_banderole(); break;
|
|
case 1: Servo_deplie_banderole(); break;
|
|
}
|
|
switch(reception[5]){
|
|
case 0: Servo_came_neutre(); break;
|
|
case 1: Servo_came_pousse(); break;
|
|
|
|
}
|
|
|
|
}
|
|
else
|
|
{
|
|
vitesse_angle = 0;
|
|
vitesse_m1 = 0;
|
|
vitesse_m2 = 0;
|
|
vitesse_m3 = 0;
|
|
vitesse_m4 = 0;
|
|
|
|
pwm_set_chan_level(4, PWM_CHAN_B, 10);
|
|
|
|
}
|
|
|
|
if(vitesse_angle != 0)
|
|
{
|
|
vitesse_m1 = vitesse_angle;
|
|
vitesse_m2 = -vitesse_angle;
|
|
vitesse_m3 = vitesse_angle;
|
|
vitesse_m4 = -vitesse_angle;
|
|
}
|
|
|
|
printf(">vitesseM1:%d\n", vitesse_m1);
|
|
printf(">vitesseM2:%d\n", vitesse_m2);
|
|
printf(">vitesseM3:%d\n", vitesse_m3);
|
|
printf(">vitesseM4:%d\n", vitesse_m4);
|
|
|
|
Motor1_speed(vitesse_m1);
|
|
Motor2_speed(vitesse_m2);
|
|
Motor3_speed(vitesse_m3);
|
|
Motor4_speed(vitesse_m4);
|
|
|
|
|
|
}
|
|
} |