Commits (1)
......@@ -33,66 +33,6 @@ const pid_struct_t M2006_pid_pos = { //A FAIRE
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;
//}
void motors_init(motor_t *motor, int16_t motor_rx_id, enum motors mtr){
......@@ -132,21 +72,25 @@ void motors_all_init(void){
motors_init(&m3508_backright, 0x203, M3508);
motors_init(&m3508_backleft, 0x204, M3508);
// Paramtres 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;
// 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;
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;
break;
case 2: // Paramtres 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;
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; //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);
}
......@@ -50,23 +54,23 @@ void run_motor_from_command(motor_t *motor, pid_struct_t pid_param, float target
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);
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;
}
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;
}
}
......@@ -92,7 +96,7 @@ void set_motor_speed(motor_t *motor)
float target = motor->target;
switch(motor->type){
case M3508:
if ((target < 100.0) && (target > -100.0)){
if ((target < 100) && (target > -100)){
run_motor_from_command(motor, motor->pid_pos, target, motor->info.M3508_info.angle);
}
else{
......@@ -106,3 +110,62 @@ void set_motor_speed(motor_t *motor)
bsp_error_handler(__FUNCTION__, __LINE__, "motor type undefined");*/
}
}
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 */
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.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;}
}
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 1 /*set 1 for Standard mca (homemade)
2 for Standard ICRA*/
enum motors {
/* can motors */
M3508,
......@@ -98,5 +101,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