Commits (1)
......@@ -3,12 +3,15 @@
#include "list_motor.h"
motor_t gm6020;
motor_t gm6020_up;
motor_t gm6020_down;
motor_t m3508_frontleft;
motor_t m3508_frontright;
motor_t m3508_backleft;
motor_t m3508_backright;
motor_t m2006;
const pid_struct_t M3508_pid_speed = { //A VERIFIER
1, 2, 0.005, 2, 16000, 16000
};
......@@ -31,13 +34,13 @@ const pid_struct_t M2006_pid_pos = { //A FAIRE
0, 12, 3, 5, 10000, 10000
};
void motors_init(void){
gm6020.type = GM6020;
void motors_all_init(void){
/*gm6020.type = GM6020;
gm6020.can_info.can_id = 0;
gm6020.can_info.tx_id = 1;
gm6020.can_info.rx_id = 0x205;
gm6020.pid_pos = GM6020_pid_pos;
gm6020.pid_speed = GM6020_pid_speed;
gm6020.pid_speed = GM6020_pid_speed;*/
m3508_frontleft.type = M3508;
m3508_frontleft.can_info.can_id = 0;
......@@ -73,5 +76,66 @@ void motors_init(void){
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;
}
/*
void motors_init(motor_t *motor, int16_t motor_rx_id, enum motors mtr){
switch (mtr){
case GM6020:
motor->type = GM6020 ;
motor->pid_pos = GM6020_pid_pos;
motor->pid_speed = GM6020_pid_speed;
motor->can_info.tx_id = 1;
break;
case M3508:
motor->type = M3508 ;
motor->pid_pos = M3508_pid_pos;
motor->pid_speed = M3508_pid_speed;
motor->can_info.tx_id = 0;
break;
case M2006:
motor->type = M2006 ;
motor->pid_pos = M2006_pid_pos;
motor->pid_speed = M2006_pid_speed;
motor->can_info.tx_id = 1;
break;
default :
break;
}
motor->can_info.can_id = 0;
motor->can_info.rx_id = motor_rx_id ;
}
void motors_all_init(void){
motors_init(&m2006, 0x207, M2006);
motors_init(&gm6020_down, 0x206, GM6020);
motors_init(&gm6020_up, 0x205, GM6020);
motors_init(&m3508_frontleft, 0x204, M3508);
motors_init(&m3508_frontright, 0x203, M3508);
motors_init(&m3508_backleft, 0x202, M3508);
motors_init(&m3508_backright, 0x201, M3508);
}
*/
......@@ -18,4 +18,5 @@ extern const pid_struct_t M3508_pid_pos;
extern const pid_struct_t GM6020_pid_speed;
extern const pid_struct_t GM6020_pid_pos;
void motors_init(void);
void motors_init(motor_t *motor, int16_t motor_rx_id, enum motors mtr);
void motors_all_init(void);
......@@ -61,6 +61,9 @@ uint8_t get_motor_data(motor_t *motor) {
case M3508:
get_3508_data(motor, motor_buf);
return 1;
case M2006:
get_2006_data(motor, motor_buf);
return 1;
default:
return 0;
}
......@@ -75,6 +78,8 @@ void set_motor_position(motor_t *motor)
run_motor_from_command(motor, motor->pid_pos, target, motor->info.M3508_info.angle);
case GM6020:
run_motor_from_command(motor, motor->pid_pos, target, motor->info.GM6020_info.angle);
case M2006:
run_motor_from_command(motor, motor->pid_pos, target, motor->info.M2006_info.angle);
/*default:
bsp_error_handler(__FUNCTION__, __LINE__, "motor type undefined");*/
}
......@@ -87,9 +92,11 @@ void set_motor_speed(motor_t *motor)
float target = motor->target;
switch(motor->type){
case M3508:
run_motor_from_command(motor, motor->pid_speed, target, motor->info.M3508_info.speed);;
run_motor_from_command(motor, motor->pid_speed, target, motor->info.M3508_info.speed);
case GM6020:
run_motor_from_command(motor, motor->pid_speed, target, motor->info.GM6020_info.speed);;
run_motor_from_command(motor, motor->pid_speed, target, motor->info.GM6020_info.speed);
case M2006:
run_motor_from_command(motor, motor->pid_speed, target, motor->info.M2006_info.speed);
/*default:
bsp_error_handler(__FUNCTION__, __LINE__, "motor type undefined");*/
}
......
......@@ -12,6 +12,8 @@
#define CAN_RX1_START 0x201
#define CAN_RX2_START 0x205
#define MAX_BASE_SPEED_COEFF 6
enum motors {
/* can motors */
M3508,
......@@ -80,6 +82,8 @@ typedef struct
pid_struct_t pid_speed; // Controler
float target;
float voltage;
int MAX_POSITION;
int MIN_POSITION;
} motor_t;
//id:0 -> 0x200, 1 -> 0x1ff
......
......@@ -88,7 +88,7 @@ void can1_read(uint16_t id, uint8_t buf[CAN_DATA_SIZE]) {
/**
* @brief send motor control message through can bus
* @param id_range to select can control id 0x1ff or 0x2ff
* @param id_range to select can control id 0x200 or 0x1ff
* @param motor voltage 1,2,3,4 or 5,6,7
* @retval None
*/
......