Newer
Older
#include "motor.h"
#include "oled.h"
#include "pid.h"
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);
}
static int16_t correct_output(motor_t *motor) {
return motor->voltage;
}
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]);
motor->info.GM6020_info.current = (int16_t)(motor_buf[4] << 8 | motor_buf[5]);
motor->info.GM6020_info.temp = (int8_t)(motor_buf[6]);
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]);
motor->info.M3508_info.current = (int16_t)(motor_buf[4] << 8 | motor_buf[5]);
motor->info.M3508_info.temp = (int8_t)(motor_buf[6]);
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]);
motor->info.M2006_info.current = (int16_t)(motor_buf[4] << 8 | motor_buf[5]);
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);
get_6020_data(motor, motor_buf);
get_3508_data(motor, motor_buf);
case M2006:
get_2006_data(motor, motor_buf);
return 1;
default:
return 0;
}
}
void set_motor_position(motor_t *motor)
{
float target = motor->target;
switch(motor->type){
case M3508:
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");*/
}
}
void set_motor_speed(motor_t *motor)
{
float target = motor->target;
switch(motor->type){
case M3508:
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);
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");*/
}
}