Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found

Target

Select target project
  • polystar/robomaster/controle/bsp
1 result
Show changes
Commits on Source (1)
Hide whitespace changes
Inline Side-by-side
Device/list_motor.c
View file @ 7403263c
......@@ -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;
}
}
Device/motor.c
View file @ 7403263c
#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
}
Device/motor.h
View file @ 7403263c
......@@ -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