Commits (1)
......@@ -10,30 +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
};
/****************************************/
/* MOTORS INITIALISATION */
/****************************************/
void motors_init(motor_t *motor, int16_t motor_rx_id, enum motors mtr){
switch (mtr){
......@@ -74,12 +80,12 @@ void motors_all_init(void){
switch (ROBOT_ID){
case 1: // Paramtres StandardMeca
gm6020_up.MIN_POSITION = 2220;
gm6020_up.MAX_POSITION = 3840;
gm6020_up.target = 3450;
gm6020_down.MIN_POSITION = 2800;
gm6020_down.MAX_POSITION = 6700;
gm6020_down.target = 4750;
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: // Paramtres ICRA
......
......@@ -6,9 +6,21 @@
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
uint16_t tx_id;
sp1 = sp2 = sp3 = sp4 = 0;
tx_id = id;
......@@ -21,11 +33,38 @@ void set_motor_voltage(uint8_t id, motor_t *motor1, motor_t *motor2, motor_t *mo
}
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]);
......@@ -33,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]);
......@@ -40,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]);
......@@ -47,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;
......@@ -87,10 +119,12 @@ 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;
......@@ -111,6 +145,26 @@ void set_motor_speed(motor_t *motor)
}
}
/**
* @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 */
......@@ -131,20 +185,43 @@ void set_all_targets_from_rc()
{ 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 */
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]);
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;
gm6020_up.target = gm6020_up.target + rc_ctrl.rc.ch[2]*0.1;
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;}
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;}
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*/
......
......@@ -14,8 +14,8 @@
#define MAX_BASE_SPEED_COEFF 6
#define ROBOT_ID 1 /*set 1 for Standard mca (homemade)
2 for Standard ICRA*/
#define ROBOT_ID 2 /*set 1 for Standard méca (homemade)
2 for Standard ICRA */
enum motors {
/* can motors */
......@@ -77,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;
......
......@@ -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) || \
......