Commits (9)
......@@ -10,90 +10,36 @@ motor_t m3508_backleft;
motor_t m3508_backright;
motor_t m2006;
/****************************************/
/* PID PARAMETERS */
/****************************************/
const pid_struct_t M3508_pid_speed = { //A VERIFIER
const pid_struct_t M3508_pid_speed = {
1, 2, 0.005, 2, 16000, 16000
};
const pid_struct_t M3508_pid_pos = { //A VERIFIER
const pid_struct_t M3508_pid_pos = {
0, 0.1, 0.005, 0, 16000, 16000
};
const pid_struct_t GM6020_pid_speed = { //A VERIFIER
const pid_struct_t GM6020_pid_speed = {
1, 40, 10, 0, 30000, 30000
};
const pid_struct_t GM6020_pid_pos = { //A VERIFIER
const pid_struct_t GM6020_pid_pos = {
0, 12, 3, 5, 10000, 10000
};
const pid_struct_t M2006_pid_speed = { //A FAIRE
const pid_struct_t M2006_pid_speed = {
1, 1.5, 0.1, 0, 16384, 16384
};
const pid_struct_t M2006_pid_pos = { //A FAIRE
const pid_struct_t M2006_pid_pos = {
0, 12, 3, 5, 10000, 10000
};
//void motors_all_init(void){
//
// m3508_frontleft.type = M3508;
// m3508_frontleft.can_info.can_id = 0;
// m3508_frontleft.can_info.tx_id = 0;
// m3508_frontleft.can_info.rx_id = 0x201;
// m3508_frontleft.pid_pos = M3508_pid_pos;
// m3508_frontleft.pid_speed = M3508_pid_speed;
//
// m3508_frontright.type = M3508;
// m3508_frontright.can_info.can_id = 0;
// m3508_frontright.can_info.tx_id = 0;
// m3508_frontright.can_info.rx_id = 0x202;
// m3508_frontright.pid_pos = M3508_pid_pos;
// m3508_frontright.pid_speed = M3508_pid_speed;
//
// m3508_backleft.type = M3508;
// m3508_backleft.can_info.can_id = 0;
// m3508_backleft.can_info.tx_id = 0;
// m3508_backleft.can_info.rx_id = 0x203;
// m3508_backleft.pid_pos = M3508_pid_pos;
// m3508_backleft.pid_speed = M3508_pid_speed;
//
// m3508_backright.type = M3508;
// m3508_backright.can_info.can_id = 0;
// m3508_backright.can_info.tx_id = 0;
// m3508_backright.can_info.rx_id = 0x204;
// m3508_backright.pid_pos = M3508_pid_pos;
// m3508_backright.pid_speed = M3508_pid_speed;
//
// m2006.type = M2006;
// m2006.can_info.can_id = 0;
// m2006.can_info.tx_id = 1;
// m2006.can_info.rx_id = 0x207;
// m2006.pid_pos = M2006_pid_pos;
// m2006.pid_speed = M2006_pid_speed;
// gm6020_up.type = GM6020;
// gm6020_up.can_info.can_id = 0;
// gm6020_up.can_info.tx_id = 1;
// gm6020_up.can_info.rx_id = 0x205;
// gm6020_up.pid_pos = GM6020_pid_pos;
// gm6020_up.pid_speed = GM6020_pid_speed;
// gm6020_up.MIN_POSITION = 5565;
// gm6020_up.MAX_POSITION = 6675;
//
// gm6020_down.type = GM6020;
// gm6020_down.can_info.can_id = 0;
// gm6020_down.can_info.tx_id = 1;
// gm6020_down.can_info.rx_id = 0x206;
// gm6020_down.pid_pos = GM6020_pid_pos;
// gm6020_down.pid_speed = GM6020_pid_speed;
// gm6020_down.MIN_POSITION = 2519;
// gm6020_down.MAX_POSITION = 7015;
//
//
// gm6020_down.target = 4750;
// gm6020_up.target = 6200;
//}
/****************************************/
/* MOTORS INITIALISATION */
/****************************************/
void motors_init(motor_t *motor, int16_t motor_rx_id, enum motors mtr){
switch (mtr){
......@@ -129,15 +75,28 @@ void motors_all_init(void){
motors_init(&gm6020_up, 0x205, GM6020);
motors_init(&m3508_frontleft, 0x201, M3508);
motors_init(&m3508_frontright, 0x202, M3508);
motors_init(&m3508_backleft, 0x203, M3508);
motors_init(&m3508_backright, 0x204, M3508);
motors_init(&m3508_backright, 0x203, M3508);
motors_init(&m3508_backleft, 0x204, M3508);
gm6020_up.MIN_POSITION = 5565;
gm6020_up.MAX_POSITION = 6675;
gm6020_down.MIN_POSITION = 2519;
gm6020_down.MAX_POSITION = 7015;
gm6020_down.target = 4750;
gm6020_up.target = 6200;
switch (ROBOT_ID){
case 1: // Param�tres StandardMeca
gm6020_up.MIN_POSITION = 7000;
gm6020_up.MAX_POSITION = 8000;
gm6020_up.target = 7450;
gm6020_down.MIN_POSITION = -1300;
gm6020_down.MAX_POSITION = 2740;
gm6020_down.target = 660;
break;
case 2: // Param�tres ICRA
gm6020_up.MIN_POSITION = 5565;
gm6020_up.MAX_POSITION = 6675;
gm6020_up.target = 6200;
gm6020_down.MIN_POSITION = 2519;
gm6020_down.MAX_POSITION = 7015;
gm6020_down.target = 4750;
break;
}
}
#include "motor.h"
#include "list_motor.h"
#include "oled.h"
#include "pid.h"
#include "bsp_uart.h"
extern RC_ctrl_t rc_ctrl;
static int16_t correct_output(motor_t *motor) {
return motor->voltage;
}
/************************************************************/
/* CAN SEND AND RECEIVE */
/************************************************************/
/**
* @brief send its command to each motor
* @param[in] CAN ID
* @param[in] the 4 motors that use this ID (see documentation)
*/
void set_motor_voltage(uint8_t id, motor_t *motor1, motor_t *motor2, motor_t *motor3, motor_t *motor4) {
int16_t sp1, sp2, sp3, sp4;
uint16_t tx_id; //can_id tait dclar l mais pas utilis
sp1 = sp2 = sp3 = sp4 = 0;
tx_id = id;
if (motor1) { sp1 = correct_output(motor1); }
if (motor2) { sp2 = correct_output(motor2); }
if (motor3) { sp3 = correct_output(motor3); }
if (motor4) { sp4 = correct_output(motor4); }
can1_transmit(tx_id, sp1, sp2, sp3, sp4);
int16_t sp1, sp2, sp3, sp4;
uint16_t tx_id;
sp1 = sp2 = sp3 = sp4 = 0;
tx_id = id;
if (motor1) { sp1 = correct_output(motor1); }
if (motor2) { sp2 = correct_output(motor2); }
if (motor3) { sp3 = correct_output(motor3); }
if (motor4) { sp4 = correct_output(motor4); }
can1_transmit(tx_id, sp1, sp2, sp3, sp4);
}
static int16_t correct_output(motor_t *motor) {
return motor->voltage;
/**
* @brief reads the CAN buff and calls the function to get data from a motor
* @param[in] motor
*/
uint8_t get_motor_data(motor_t *motor) {
static uint8_t motor_buf[CAN_DATA_SIZE];
can1_read(motor->can_info.rx_id, motor_buf);
switch (motor->type) {
case GM6020:
get_6020_data(motor, motor_buf);
return 1;
case M3508:
get_3508_data(motor, motor_buf);
return 1;
case M2006:
get_2006_data(motor, motor_buf);
return 1;
default:
return 0;
}
}
/************************************************************/
/* GET MOTORS DATA */
/************************************************************/
/**
* @brief Get CAN data from a GM6020 motor
* @param[in] motor
* @param[in] receive buff
*/
static void get_6020_data(motor_t* motor, uint8_t motor_buf[CAN_DATA_SIZE]){
motor->info.GM6020_info.angle = (int16_t)(motor_buf[0] << 8 | motor_buf[1]);
motor->info.GM6020_info.speed = (int16_t)(motor_buf[2] << 8 | motor_buf[3]);
......@@ -29,6 +72,11 @@ static void get_6020_data(motor_t* motor, uint8_t motor_buf[CAN_DATA_SIZE]){
motor->info.GM6020_info.temp = (int8_t)(motor_buf[6]);
}
/**
* @brief Get CAN data from a M3508 motor
* @param[in] motor
* @param[in] receive buff
*/
static void get_3508_data(motor_t* motor, uint8_t motor_buf[CAN_DATA_SIZE]){
motor->info.M3508_info.angle = (int16_t)(motor_buf[0] << 8 | motor_buf[1]);
motor->info.M3508_info.speed = (int16_t)(motor_buf[2] << 8 | motor_buf[3]);
......@@ -36,6 +84,11 @@ static void get_3508_data(motor_t* motor, uint8_t motor_buf[CAN_DATA_SIZE]){
motor->info.M3508_info.temp = (int8_t)(motor_buf[6]);
}
/**
* @brief Get CAN data from a M2006 motor
* @param[in] motor
* @param[in] receive buff
*/
static void get_2006_data(motor_t* motor, uint8_t motor_buf[CAN_DATA_SIZE]){
motor->info.M2006_info.angle = (int16_t)(motor_buf[0] << 8 | motor_buf[1]);
motor->info.M2006_info.speed = (int16_t)(motor_buf[2] << 8 | motor_buf[3]);
......@@ -43,33 +96,16 @@ static void get_2006_data(motor_t* motor, uint8_t motor_buf[CAN_DATA_SIZE]){
motor->info.M2006_info.temp = (int8_t)(motor_buf[6]);
}
void run_motor_from_command(motor_t *motor, pid_struct_t pid_param, float target, float fdb)
{
motor->voltage = pid_calc(&pid_param, target, fdb);
}
uint8_t get_motor_data(motor_t *motor) {
static uint8_t motor_buf[CAN_DATA_SIZE];
can1_read(motor->can_info.rx_id, motor_buf);
switch (motor->type) {
case GM6020:
get_6020_data(motor, motor_buf);
return 1;
case M3508:
get_3508_data(motor, motor_buf);
return 1;
case M2006:
get_2006_data(motor, motor_buf);
return 1;
default:
return 0;
}
}
/************************************************************/
/* SET TARGET AND RUN MOTORS */
/************************************************************/
/**
* @brief sets the target (position) of a motor
* @param[in] motor
*/
void set_motor_position(motor_t *motor)
{
float target = motor->target;
......@@ -83,16 +119,41 @@ void set_motor_position(motor_t *motor)
/*default:
bsp_error_handler(__FUNCTION__, __LINE__, "motor type undefined");*/
}
}
/**
* @brief sets the target (speed) of a motor
* @param[in] motor
*/
void set_motor_speed(motor_t *motor)
{
float target = motor->target;
switch(motor->type){
case M3508:
<<<<<<< HEAD
<<<<<<< HEAD
if ((target < 150) && (target > -100)){
run_motor_from_command(motor, motor->pid_speed, target, motor->info.M3508_info.speed);
}
else{
run_motor_from_command(motor, motor->pid_pos, target, motor->info.M3508_info.angle);
}
=======
if ((target < 100.0) && (target > -100.0)){
run_motor_from_command(motor, motor->pid_pos, target, motor->info.M3508_info.angle);
}
else{
run_motor_from_command(motor, motor->pid_speed, target, motor->info.M3508_info.speed);
}
>>>>>>> 623e9ad5906292f23a358f1ea43fd89ee37f30d3
=======
if ((target < 100) && (target > -100)){
run_motor_from_command(motor, motor->pid_pos, target, motor->info.M3508_info.angle);
}
else{
run_motor_from_command(motor, motor->pid_speed, target, motor->info.M3508_info.speed);
}
>>>>>>> position_M3508_BSP
case GM6020:
run_motor_from_command(motor, motor->pid_speed, target, motor->info.GM6020_info.speed);
case M2006:
......@@ -101,3 +162,105 @@ void set_motor_speed(motor_t *motor)
bsp_error_handler(__FUNCTION__, __LINE__, "motor type undefined");*/
}
}
/**
* @brief sets the command (voltage) of each motor after PID calculated it
* @param[in] motor
* @param[in] PID parameters of the motor
* @param[in] target (speed or position)
* @param[in] feedback (speed or angle) from the motor
*/
void run_motor_from_command(motor_t *motor, pid_struct_t pid_param, float target, float fdb)
{
motor->voltage = pid_calc(&pid_param, target, fdb);
}
/************************************************************/
/* GLOBAL RUNNING FUNCTIONS */
/************************************************************/
/**
* @brief sets all targets from remote controler command
*/
void set_all_targets_from_rc()
{
/* Switches activation */
// Switch 1 : m2006 motor (feeder)
if (rc_ctrl.rc.sw[0] == 2)
{ m2006.target = 0; };
if (rc_ctrl.rc.sw[0] == 3)
{ m2006.target = 2000; };
if (rc_ctrl.rc.sw[0] == 1)
{ m2006.target = 5000; };
//Switch 2 : snails (shoot)
if (rc_ctrl.rc.sw[1] == 2)
{ PWM_SetAllDuty(&htim1, 0.0f, 0.0f, 0.0f, 0.0f); };
if (rc_ctrl.rc.sw[1] == 3)
{ PWM_SetAllDuty(&htim1, 0.25f, 0.25f, 0.25f, 0.25f); };
if (rc_ctrl.rc.sw[1] == 1)
{ PWM_SetAllDuty(&htim1, 0.5f, 0.5f, 0.5f, 0.5f); };
/* Setting the target for all motors according to the data received from the remote controler */
switch (ROBOT_ID){
case 1:
m3508_frontleft.target = MAX_BASE_SPEED_COEFF*(rc_ctrl.rc.ch[0] + rc_ctrl.rc.ch[1] - rc_ctrl.rc.ch[4]);
m3508_frontright.target = -MAX_BASE_SPEED_COEFF*(rc_ctrl.rc.ch[0] - rc_ctrl.rc.ch[1] + rc_ctrl.rc.ch[4]);
m3508_backright.target = -MAX_BASE_SPEED_COEFF*(rc_ctrl.rc.ch[0] + rc_ctrl.rc.ch[1] + rc_ctrl.rc.ch[4]);
m3508_backleft.target = MAX_BASE_SPEED_COEFF*(rc_ctrl.rc.ch[0] - rc_ctrl.rc.ch[1] - rc_ctrl.rc.ch[4]);
case 2:
m3508_frontleft.target = MAX_BASE_SPEED_COEFF*(rc_ctrl.rc.ch[0] - rc_ctrl.rc.ch[1] - rc_ctrl.rc.ch[4]);
m3508_frontright.target = -MAX_BASE_SPEED_COEFF*(rc_ctrl.rc.ch[0] + rc_ctrl.rc.ch[1] + rc_ctrl.rc.ch[4]);
m3508_backright.target = -MAX_BASE_SPEED_COEFF*(rc_ctrl.rc.ch[0] - rc_ctrl.rc.ch[1] + rc_ctrl.rc.ch[4]);
m3508_backleft.target = MAX_BASE_SPEED_COEFF*(rc_ctrl.rc.ch[0] + rc_ctrl.rc.ch[1] - rc_ctrl.rc.ch[4]);
}
gm6020_down.target = gm6020_down.target - rc_ctrl.rc.ch[3]*0.1;
gm6020_up.target = gm6020_up.target + rc_ctrl.rc.ch[2]*0.1;
if (gm6020_down.MAX_POSITION > gm6020_down.MIN_POSITION){
if (gm6020_down.target > gm6020_down.MAX_POSITION){gm6020_down.target = gm6020_down.MAX_POSITION;}
if (gm6020_down.target < gm6020_down.MIN_POSITION){gm6020_down.target = gm6020_down.MIN_POSITION;}
}
else{
if (gm6020_down.target > gm6020_down.MAX_POSITION && gm6020_down.target < gm6020_down.MAX_POSITION+1000){gm6020_down.target = gm6020_down.MAX_POSITION;}
if (gm6020_down.target < gm6020_down.MIN_POSITION && gm6020_down.target > gm6020_down.MIN_POSITION-1000){gm6020_down.target = gm6020_down.MIN_POSITION;}
}
if (gm6020_up.MAX_POSITION > gm6020_up.MIN_POSITION){
if (gm6020_up.target > gm6020_up.MAX_POSITION){gm6020_up.target = gm6020_up.MAX_POSITION;}
if (gm6020_up.target < gm6020_up.MIN_POSITION){gm6020_up.target = gm6020_up.MIN_POSITION;}
}
else{
if (gm6020_up.target > gm6020_up.MAX_POSITION && gm6020_up.target < gm6020_up.MAX_POSITION+1000){gm6020_up.target = gm6020_up.MAX_POSITION;}
if (gm6020_up.target < gm6020_up.MIN_POSITION && gm6020_up.target > gm6020_up.MIN_POSITION-1000){gm6020_up.target = gm6020_up.MIN_POSITION;}
}
}
/**
* @brief gets the data from each motor and execute the function to run it
*/
void command_all_motors()
{
/* Get data from all motors*/
get_motor_data(&m3508_frontleft);
get_motor_data(&m3508_frontright);
get_motor_data(&m3508_backleft);
get_motor_data(&m3508_backright);
get_motor_data(&m2006);
get_motor_data(&gm6020_up);
get_motor_data(&gm6020_down);
/* Calculate the command to be sent */
set_motor_speed(&m3508_frontleft);
set_motor_speed(&m3508_frontright);
set_motor_speed(&m3508_backleft);
set_motor_speed(&m3508_backright);
set_motor_speed(&m2006);
set_motor_position(&gm6020_up);
set_motor_position(&gm6020_down);
/* Transmit the command to the motors */
set_motor_voltage(0, &m3508_frontleft, &m3508_frontright, &m3508_backright, &m3508_backleft); //0x200
set_motor_voltage(1, &gm6020_up, &gm6020_down, &m2006, NULL); //0x1ff
}
......@@ -14,6 +14,9 @@
#define MAX_BASE_SPEED_COEFF 6
#define ROBOT_ID 2 /*set 1 for Standard méca (homemade)
2 for Standard ICRA */
enum motors {
/* can motors */
M3508,
......@@ -74,12 +77,11 @@ typedef union{
// Une struct par moteur
typedef struct
{
can_info_t can_info; // Can ID information
//M3508_info_t info;
motor_info_t info; // Specific info about the motor
int type; // Type du moteur, ex:GM6020
can_info_t can_info; // Can ID information
motor_info_t info; // Specific info about the motor
int type; // Type du moteur, ex:GM6020
pid_struct_t pid_pos; // Controler
pid_struct_t pid_speed; // Controler
pid_struct_t pid_speed; // Controler
float target;
float voltage;
int MAX_POSITION;
......@@ -98,5 +100,7 @@ static void get_2006_data(motor_t* motor, uint8_t buf[CAN_DATA_SIZE]);
void set_motor_position(motor_t *motor);
void set_motor_speed(motor_t *motor);
void run_motor_from_command(motor_t *motor, pid_struct_t pid_param, float target, float fdb);
void set_all_targets_from_rc(void);
void command_all_motors(void);
#endif
......@@ -100,9 +100,8 @@ void rc_callback_handler(RC_ctrl_t *rc_ctrl, uint8_t *buff)
rc_ctrl->mouse.press_l = buff[12]; //!< Mouse Left Is Press ?
rc_ctrl->mouse.press_r = buff[13]; //!< Mouse Right Is Press ?
rc_ctrl->key.v = buff[14] | (buff[15] << 8); //!< KeyBoard value
rc_ctrl->rc.ch[4] = buff[16] | (buff[17] << 8); //!< Left Dial
rc_ctrl->rc.ch[4] = buff[16] | (buff[17] << 8); //!< Left Dial
rc_ctrl->rc.ch[4] -= 1024;
if ((abs(rc_ctrl->rc.ch[0]) > 660) || \
(abs(rc_ctrl->rc.ch[1]) > 660) || \
......