From 17b37910877054ef2cce1005b87478e51002acca Mon Sep 17 00:00:00 2001 From: Soil_L Date: Thu, 14 Nov 2019 22:00:56 +0800 Subject: [PATCH 1/5] feat:add motor and encoder based can-bus and add a kinematics --- encoder/can_encoder.c | 74 +++++++++++++++++++++++++++++++++++ encoder/can_encoder.h | 19 +++++++++ encoder/encoder.c | 14 +++++-- kinematics/kinematics.c | 60 +++++++++++++++++++++------- kinematics/kinematics.h | 3 +- motor/can_motor.c | 87 +++++++++++++++++++++++++++++++++++++++++ motor/can_motor.h | 21 ++++++++++ 7 files changed, 258 insertions(+), 20 deletions(-) create mode 100644 encoder/can_encoder.c create mode 100644 encoder/can_encoder.h create mode 100644 motor/can_motor.c create mode 100644 motor/can_motor.h diff --git a/encoder/can_encoder.c b/encoder/can_encoder.c new file mode 100644 index 0000000..f981fd3 --- /dev/null +++ b/encoder/can_encoder.c @@ -0,0 +1,74 @@ +#include "can_encoder.h" + +#define DBG_SECTION_NAME "can_encoder" +#define DBG_LEVEL DBG_LOG +#include + +rt_sem_t rx_sem_g; + +rt_err_t encoder_isr(rt_device_t dev, rt_size_t size) +{ + rt_sem_release(rx_sem_g); + return RT_EOK; +} + +static rt_err_t can_encoder_enable(void *enc) +{ + can_encoder_t enc_sub = (can_encoder_t)enc; + + volatile rt_err_t res; + + //Avoid repeated opening + if(!(enc_sub->can_dev->flag & RT_DEVICE_FLAG_ACTIVATED)) + { + res = rt_device_open(enc_sub->can_dev,RT_DEVICE_FLAG_INT_RX|RT_DEVICE_FLAG_INT_TX); + res = rt_device_control(enc_sub->can_dev,RT_CAN_CMD_SET_BAUD,(void *)enc_sub->Baud); + res = rt_device_control(enc_sub->can_dev,RT_CAN_CMD_SET_FILTER,&enc_sub->cfg); + } + res = rt_device_set_rx_indicate(enc_sub->can_dev,encoder_isr); + + return RT_EOK; +} + +static rt_err_t can_encoder_disable(void *enc) +{ + return RT_EOK; +} + +static rt_err_t can_encoder_destroy(void *enc) +{ + can_encoder_t enc_sub = (can_encoder_t)enc; + + volatile rt_err_t res; + res = rt_device_close(enc_sub->can_dev); + rt_sem_delete(*(enc_sub->rx_sem)); + rt_free(enc_sub); + + return RT_EOK; +} + +can_encoder_t can_encoder_create(char *can,struct rt_can_filter_config cfg,rt_uint32_t Baud,rt_sem_t* rx_sem) +{ + can_encoder_t new_encoder = (can_encoder_t)encoder_create(sizeof(struct can_encoder),0); + if(new_encoder == RT_NULL) + { + return RT_NULL; + } + + new_encoder->can_dev = (rt_device_t)rt_device_find(can); + if (new_encoder->can_dev == RT_NULL) + { + rt_free(new_encoder); + LOG_E("Falied to find device on %s", can); + return RT_NULL; + } + + new_encoder->cfg = cfg; + new_encoder->enc.enable = can_encoder_enable; + new_encoder->enc.disable = can_encoder_disable; + new_encoder->enc.destroy = can_encoder_destroy; + new_encoder->Baud = Baud; + new_encoder->rx_sem = rx_sem; + rx_sem_g = *rx_sem; + return new_encoder; +} diff --git a/encoder/can_encoder.h b/encoder/can_encoder.h new file mode 100644 index 0000000..5fdde60 --- /dev/null +++ b/encoder/can_encoder.h @@ -0,0 +1,19 @@ +#ifndef _RT_ROBOT_CAN_ENCODER_ +#define _RT_ROBOT_CAN_ENCODER_ + +#include "encoder.h" + +struct can_encoder +{ + struct encoder enc; + rt_device_t can_dev; + struct rt_can_filter_config cfg; + rt_uint32_t Baud; + rt_sem_t *rx_sem; + rt_uint32_t rpm; +}; +typedef struct can_encoder *can_encoder_t; + +can_encoder_t can_encoder_create(char *can,struct rt_can_filter_config cfg,rt_uint32_t Baud,rt_sem_t* rx_sem); + +#endif //CAN_ENCODER_ diff --git a/encoder/encoder.c b/encoder/encoder.c index b3acf2b..5063468 100644 --- a/encoder/encoder.c +++ b/encoder/encoder.c @@ -96,10 +96,16 @@ rt_int16_t encoder_measure_cps(encoder_t enc) rt_int16_t encoder_measure_rpm(encoder_t enc) { RT_ASSERT(enc != RT_NULL); - - // return resolution per minute - rt_int16_t res_rpm = encoder_measure_cps(enc) * 60 / enc->pulse_revol; - + rt_int16_t res_rpm; + if(enc->sample_time == 0) + { + res_rpm = enc->pulse_count; + } + else + { + // return resolution per minute + res_rpm = encoder_measure_cps(enc) * 60 / enc->pulse_revol; + } return res_rpm; } diff --git a/kinematics/kinematics.c b/kinematics/kinematics.c index 306c5b2..3a12ed1 100644 --- a/kinematics/kinematics.c +++ b/kinematics/kinematics.c @@ -44,6 +44,10 @@ kinematics_t kinematics_create(enum base k_base, float length_x, float length_y, { new_kinematics->total_wheels = 4; } + if(k_base == FOUR_WD_ALLDIR) + { + new_kinematics->total_wheels = 4; + } return new_kinematics; } @@ -134,6 +138,13 @@ void kinematics_get_rpm(struct kinematics kin, struct velocity target_vel, rt_in res_rpm[2] = cal_rpm.motor3; res_rpm[3] = cal_rpm.motor4; } + else if(kin.k_base == FOUR_WD_ALLDIR) //FRONT:0 BACK:1 LEFT:2 RIGHT:3 + { + res_rpm[0] = x_rpm + tan_rpm; + res_rpm[1] = -x_rpm + tan_rpm; + res_rpm[2] = y_rpm + tan_rpm; + res_rpm[3] = -y_rpm + tan_rpm; + } else { return; @@ -156,25 +167,44 @@ void kinematics_get_velocity(struct kinematics kin, struct rpm current_rpm, stru if(kin.k_base == FOUR_WD) total_wheels = 4; if(kin.k_base == ACKERMANN) total_wheels = 2; if(kin.k_base == MECANUM) total_wheels = 4; - + if(kin.k_base == FOUR_WD_ALLDIR) total_wheels = 4; + float average_rps_x; float average_rps_y; float average_rps_a; - //convert average revolutions per minute to revolutions per second - average_rps_x = ((float)(current_rpm.motor1 + current_rpm.motor2 + current_rpm.motor3 + current_rpm.motor4) / total_wheels) / 60; // RPM - res_vel.linear_x = average_rps_x * kin.wheel_radius; // m/s - - //convert average revolutions per minute in y axis to revolutions per second - average_rps_y = ((float)(-current_rpm.motor1 + current_rpm.motor2 + current_rpm.motor3 - current_rpm.motor4) / total_wheels) / 60; // RPM - if(kin.k_base == MECANUM) - res_vel.linear_y = average_rps_y * kin.wheel_radius; // m/s - else - res_vel.linear_y = 0; - - //convert average revolutions per minute to revolutions per second - average_rps_a = ((float)(-current_rpm.motor1 + current_rpm.motor2 - current_rpm.motor3 + current_rpm.motor4) / total_wheels) / 60; - res_vel.angular_z = (average_rps_a * kin.wheel_radius) / ((kin.length_x / 2) + (kin.length_y / 2)); // rad/s + if(kin.k_base == TWO_WD || kin.k_base == FOUR_WD || kin.k_base == ACKERMANN) + { + //convert average revolutions per minute to revolutions per second + average_rps_x = ((float)(current_rpm.motor1 + current_rpm.motor2 + current_rpm.motor3 + current_rpm.motor4) / total_wheels) / 60; // RPM + res_vel.linear_x = average_rps_x * kin.wheel_radius; // m/s + + //convert average revolutions per minute in y axis to revolutions per second + average_rps_y = ((float)(-current_rpm.motor1 + current_rpm.motor2 + current_rpm.motor3 - current_rpm.motor4) / total_wheels) / 60; // RPM + if(kin.k_base == MECANUM) + res_vel.linear_y = average_rps_y * kin.wheel_radius; // m/s + else + res_vel.linear_y = 0; + + //convert average revolutions per minute to revolutions per second + average_rps_a = ((float)(-current_rpm.motor1 + current_rpm.motor2 - current_rpm.motor3 + current_rpm.motor4) / total_wheels) / 60; + res_vel.angular_z = (average_rps_a * kin.wheel_radius) / ((kin.length_x / 2) + (kin.length_y / 2)); // rad/s + } + if(kin.k_base == FOUR_WD_ALLDIR)//FRONT:motor1 BACK:motor2 LEFT:motor3 RIGHT:motor4 + { + //convert average revolutions per minute to revolutions per second + average_rps_x = ((float)(current_rpm.motor1 - current_rpm.motor2) / 2) / 60; // RPM + res_vel.linear_x = average_rps_x * kin.total_wheels; // m/s + + //convert average revolutions per minute in y axis to revolutions per second + average_rps_y = ((float)(current_rpm.motor3 - current_rpm.motor4) / 2) / 60; // RPM + res_vel.linear_y = average_rps_y * kin.wheel_radius; // m/s + + //convert average revolutions per minute to revolutions per second + average_rps_a = ((float)(current_rpm.motor1 + current_rpm.motor2 + current_rpm.motor3 + current_rpm.motor4 + - 2*60*(average_rps_x+average_rps_y)) / total_wheels) / 60; + res_vel.angular_z = (average_rps_a * kin.wheel_radius) / ((kin.length_x / 2) + (kin.length_y / 2)); // rad/s + } rt_memcpy(velocity, &res_vel, sizeof(struct velocity)); } diff --git a/kinematics/kinematics.h b/kinematics/kinematics.h index 67cd18d..3e10b7a 100644 --- a/kinematics/kinematics.h +++ b/kinematics/kinematics.h @@ -17,7 +17,8 @@ enum base { TWO_WD = 0, FOUR_WD, ACKERMANN, - MECANUM + MECANUM, + FOUR_WD_ALLDIR }; // rad/min diff --git a/motor/can_motor.c b/motor/can_motor.c new file mode 100644 index 0000000..613d515 --- /dev/null +++ b/motor/can_motor.c @@ -0,0 +1,87 @@ +#include "can_motor.h" + +#define DBG_SECTION_NAME "can_motor" +#define DBG_LEVEL DBG_LOG +#include + + +static rt_err_t can_motor_enable(void *mot) +{ + can_motor_t mot_sub = (can_motor_t)mot; + + //Avoid repeated opening + if(!(mot_sub->can_dev->flag & RT_DEVICE_FLAG_ACTIVATED)) + { + rt_err_t res = rt_device_open(mot_sub->can_dev,RT_DEVICE_FLAG_INT_TX | RT_DEVICE_FLAG_INT_RX ); + res = rt_device_control(mot_sub->can_dev,RT_CAN_CMD_SET_BAUD,(void *)mot_sub->Baud); + res = rt_device_control(mot_sub->can_dev,RT_CAN_CMD_SET_FILTER,&mot_sub->cfg); + } + return RT_EOK; +} + +static rt_err_t can_motor_disable(void *mot) +{ + can_motor_t mot_sub = (can_motor_t)mot; + + rt_err_t res = rt_device_close(mot_sub->can_dev); + + return RT_EOK; +} + +static rt_err_t can_motor_reset(void *mot) +{ + return RT_EOK; +} + +static rt_err_t can_motor_set_speed(void *mot, rt_int16_t thousands) +{ + can_motor_t mot_sub = (can_motor_t)mot; + + if(mot_sub->min_num > thousands) + { + thousands = mot_sub->min_num; + } + else if(mot_sub->max_num < thousands) + { + thousands = mot_sub->max_num; + } + + mot_sub->msg->data[(mot_sub->mot_id -1)*2] = thousands>>8; + mot_sub->msg->data[(mot_sub->mot_id -1)*2+1] = thousands; + rt_size_t size = rt_device_write(mot_sub->can_dev, 0, mot_sub->msg, sizeof(*(mot_sub->msg)) ); + + return RT_EOK; +} + + +can_motor_t can_motor_create(char *can,struct rt_can_filter_config cfg,rt_uint32_t Baud,rt_can_msg_t msg,rt_int16_t max_num,rt_int16_t min_num,rt_uint8_t mot_id) +{ + RT_ASSERT(max_num>min_num) + + can_motor_t new_motor = (can_motor_t)motor_create(sizeof(struct can_motor)); + if (new_motor == RT_NULL) + { + return RT_NULL; + } + + new_motor->can_dev = (rt_device_t)rt_device_find(can); + if (new_motor->can_dev == RT_NULL) + { + rt_free(new_motor); + LOG_E("Falied to find device on %s", can); + return RT_NULL; + } + + new_motor->cfg = cfg; + new_motor->Baud = Baud; + new_motor->msg = msg; + new_motor->max_num = max_num; + new_motor->min_num = min_num; + new_motor->mot_id = mot_id; + new_motor->mot.enable = can_motor_enable; + new_motor->mot.set_speed = can_motor_set_speed; + new_motor->mot.reset = can_motor_reset; + new_motor->mot.disable = can_motor_disable; + + return new_motor; +} diff --git a/motor/can_motor.h b/motor/can_motor.h new file mode 100644 index 0000000..5741a23 --- /dev/null +++ b/motor/can_motor.h @@ -0,0 +1,21 @@ +#ifndef _CAN_MOTOR_H_ +#define _CAN_MOTOR_H_ + +#include "motor.h" + +struct can_motor +{ + struct motor mot; + rt_device_t can_dev; + struct rt_can_filter_config cfg; + rt_uint32_t Baud; + rt_can_msg_t msg; //the pointer of message defined by the users,it decide the number of the can motor in the can net + rt_int16_t max_num; + rt_int16_t min_num; + rt_uint8_t mot_id; //the id of the can motor in the can net +}; +typedef struct can_motor *can_motor_t; + +can_motor_t can_motor_create(char *can,struct rt_can_filter_config cfg,rt_uint32_t Baud,rt_can_msg_t msg,rt_int16_t max_num,rt_int16_t min_num,rt_uint8_t mot_id); + +#endif // __CAN_MOTOR_H__ From ee534791c593a2b4e0731f5e497ee7f38e93a0c4 Mon Sep 17 00:00:00 2001 From: Soil_L Date: Sat, 18 Jan 2020 21:44:41 +0800 Subject: [PATCH 2/5] refactor:replace the tabs with spaces --- encoder/can_encoder.c | 72 ++++++++++----------- encoder/can_encoder.h | 10 +-- encoder/encoder.c | 48 +++++++------- kinematics/kinematics.c | 138 ++++++++++++++++++++-------------------- kinematics/kinematics.h | 2 +- motor/can_motor.c | 84 ++++++++++++------------ motor/can_motor.h | 14 ++-- 7 files changed, 184 insertions(+), 184 deletions(-) diff --git a/encoder/can_encoder.c b/encoder/can_encoder.c index f981fd3..d6351fb 100644 --- a/encoder/can_encoder.c +++ b/encoder/can_encoder.c @@ -8,25 +8,25 @@ rt_sem_t rx_sem_g; rt_err_t encoder_isr(rt_device_t dev, rt_size_t size) { - rt_sem_release(rx_sem_g); - return RT_EOK; + rt_sem_release(rx_sem_g); + return RT_EOK; } static rt_err_t can_encoder_enable(void *enc) { can_encoder_t enc_sub = (can_encoder_t)enc; - - volatile rt_err_t res; - - //Avoid repeated opening - if(!(enc_sub->can_dev->flag & RT_DEVICE_FLAG_ACTIVATED)) - { - res = rt_device_open(enc_sub->can_dev,RT_DEVICE_FLAG_INT_RX|RT_DEVICE_FLAG_INT_TX); - res = rt_device_control(enc_sub->can_dev,RT_CAN_CMD_SET_BAUD,(void *)enc_sub->Baud); - res = rt_device_control(enc_sub->can_dev,RT_CAN_CMD_SET_FILTER,&enc_sub->cfg); - } - res = rt_device_set_rx_indicate(enc_sub->can_dev,encoder_isr); - + + volatile rt_err_t res; + + //Avoid repeated opening + if(!(enc_sub->can_dev->flag & RT_DEVICE_FLAG_ACTIVATED)) + { + res = rt_device_open(enc_sub->can_dev,RT_DEVICE_FLAG_INT_RX|RT_DEVICE_FLAG_INT_TX); + res = rt_device_control(enc_sub->can_dev,RT_CAN_CMD_SET_BAUD,(void *)enc_sub->Baud); + res = rt_device_control(enc_sub->can_dev,RT_CAN_CMD_SET_FILTER,&enc_sub->cfg); + } + res = rt_device_set_rx_indicate(enc_sub->can_dev,encoder_isr); + return RT_EOK; } @@ -39,36 +39,36 @@ static rt_err_t can_encoder_destroy(void *enc) { can_encoder_t enc_sub = (can_encoder_t)enc; - volatile rt_err_t res; - res = rt_device_close(enc_sub->can_dev); - rt_sem_delete(*(enc_sub->rx_sem)); + volatile rt_err_t res; + res = rt_device_close(enc_sub->can_dev); + rt_sem_delete(*(enc_sub->rx_sem)); rt_free(enc_sub); - + return RT_EOK; } can_encoder_t can_encoder_create(char *can,struct rt_can_filter_config cfg,rt_uint32_t Baud,rt_sem_t* rx_sem) { - can_encoder_t new_encoder = (can_encoder_t)encoder_create(sizeof(struct can_encoder),0); - if(new_encoder == RT_NULL) - { + can_encoder_t new_encoder = (can_encoder_t)encoder_create(sizeof(struct can_encoder),0); + if(new_encoder == RT_NULL) + { return RT_NULL; - } - - new_encoder->can_dev = (rt_device_t)rt_device_find(can); - if (new_encoder->can_dev == RT_NULL) - { + } + + new_encoder->can_dev = (rt_device_t)rt_device_find(can); + if (new_encoder->can_dev == RT_NULL) + { rt_free(new_encoder); LOG_E("Falied to find device on %s", can); return RT_NULL; - } - - new_encoder->cfg = cfg; - new_encoder->enc.enable = can_encoder_enable; - new_encoder->enc.disable = can_encoder_disable; - new_encoder->enc.destroy = can_encoder_destroy; - new_encoder->Baud = Baud; - new_encoder->rx_sem = rx_sem; - rx_sem_g = *rx_sem; - return new_encoder; + } + + new_encoder->cfg = cfg; + new_encoder->enc.enable = can_encoder_enable; + new_encoder->enc.disable = can_encoder_disable; + new_encoder->enc.destroy = can_encoder_destroy; + new_encoder->Baud = Baud; + new_encoder->rx_sem = rx_sem; + rx_sem_g = *rx_sem; + return new_encoder; } diff --git a/encoder/can_encoder.h b/encoder/can_encoder.h index f934eca..0323a8d 100644 --- a/encoder/can_encoder.h +++ b/encoder/can_encoder.h @@ -5,12 +5,12 @@ struct can_encoder { - struct encoder enc; - rt_device_t can_dev; + struct encoder enc; + rt_device_t can_dev; struct rt_can_filter_config cfg; - rt_uint32_t Baud; - rt_sem_t *rx_sem; - rt_uint32_t rpm; + rt_uint32_t Baud; + rt_sem_t *rx_sem; + rt_uint32_t rpm; }; typedef struct can_encoder *can_encoder_t; diff --git a/encoder/encoder.c b/encoder/encoder.c index 5063468..262889d 100644 --- a/encoder/encoder.c +++ b/encoder/encoder.c @@ -21,14 +21,14 @@ encoder_t encoder_create(rt_size_t size, rt_uint16_t sample_time) encoder_t new_encoder = (encoder_t)rt_malloc(size); if(new_encoder == RT_NULL) { - LOG_E("Failed to malloc memory for new encoder\n"); - return RT_NULL; + LOG_E("Failed to malloc memory for new encoder\n"); + return RT_NULL; } - + new_encoder->pulse_count = 0; new_encoder->last_count = 0; new_encoder->sample_time = sample_time; - + return new_encoder; } @@ -36,7 +36,7 @@ rt_err_t encoder_destroy(encoder_t enc) { LOG_D("Free Encoder"); RT_ASSERT(enc != RT_NULL); - + return enc->destroy(enc); } @@ -44,7 +44,7 @@ rt_err_t encoder_enable(encoder_t enc) { LOG_D("Enabling encoder"); RT_ASSERT(enc != RT_NULL); - + return enc->enable(enc); } @@ -52,24 +52,24 @@ rt_err_t encoder_disable(encoder_t enc) { LOG_D("Diabling encoder"); RT_ASSERT(enc != RT_NULL); - + return enc->disable(enc); } rt_int32_t encoder_read(encoder_t enc) { RT_ASSERT(enc != RT_NULL); - + return enc->pulse_count; } rt_err_t encoder_reset(encoder_t enc) { RT_ASSERT(enc != RT_NULL); - + enc->pulse_count = 0; enc->last_count = 0; - + return RT_EOK; } @@ -80,16 +80,16 @@ rt_int16_t encoder_measure_cps(encoder_t enc) // return count per second if((rt_tick_get() - enc->last_time) < rt_tick_from_millisecond(enc->sample_time)) { - LOG_D("Encoder waiting ... "); - return enc->cps; + LOG_D("Encoder waiting ... "); + return enc->cps; } - + rt_int32_t diff_count = enc->pulse_count - enc->last_count; - + enc->cps = diff_count * 1000 / enc->sample_time; enc->last_count = enc->pulse_count; enc->last_time = rt_tick_get(); - + return enc->cps; } @@ -97,15 +97,15 @@ rt_int16_t encoder_measure_rpm(encoder_t enc) { RT_ASSERT(enc != RT_NULL); rt_int16_t res_rpm; - if(enc->sample_time == 0) - { - res_rpm = enc->pulse_count; - } - else - { - // return resolution per minute - res_rpm = encoder_measure_cps(enc) * 60 / enc->pulse_revol; - } + if(enc->sample_time == 0) + { + res_rpm = enc->pulse_count; + } + else + { + // return resolution per minute + res_rpm = encoder_measure_cps(enc) * 60 / enc->pulse_revol; + } return res_rpm; } diff --git a/kinematics/kinematics.c b/kinematics/kinematics.c index 4ad3deb..9a49ca8 100644 --- a/kinematics/kinematics.c +++ b/kinematics/kinematics.c @@ -21,36 +21,36 @@ kinematics_t kinematics_create(enum base k_base, float length_x, float length_y, kinematics_t new_kinematics = (kinematics_t)rt_malloc(sizeof(struct kinematics)); if(new_kinematics == RT_NULL) { - LOG_E("Failed to malloc memory for kinematics\n"); - return RT_NULL; + LOG_E("Failed to malloc memory for kinematics\n"); + return RT_NULL; } - + new_kinematics->k_base = k_base; new_kinematics->length_x = length_x; new_kinematics->length_y = length_y; new_kinematics->wheel_cir = wheel_radius * 2.0f * PI;; - + if(k_base == TWO_WD) { - new_kinematics->total_wheels = 2; + new_kinematics->total_wheels = 2; } if(k_base == FOUR_WD) { - new_kinematics->total_wheels = 4; + new_kinematics->total_wheels = 4; } if(k_base == ACKERMANN) { - new_kinematics->total_wheels = 3; + new_kinematics->total_wheels = 3; } if(k_base == MECANUM) { - new_kinematics->total_wheels = 4; + new_kinematics->total_wheels = 4; } - if(k_base == FOUR_WD_ALLDIR) - { - new_kinematics->total_wheels = 4; - } - + if(k_base == FOUR_WD_ALLDIR) + { + new_kinematics->total_wheels = 4; + } + return new_kinematics; } @@ -75,78 +75,78 @@ void kinematics_get_rpm(struct kinematics kin, struct velocity target_vel, rt_in // TODO struct rpm cal_rpm; rt_int16_t res_rpm[4] = {0}; - + float linear_vel_x_mins; float linear_vel_y_mins; float angular_vel_z_mins; float tangential_vel; - + float x_rpm; float y_rpm; float tan_rpm; if(kin.k_base == TWO_WD || kin.k_base == FOUR_WD || kin.k_base == ACKERMANN) { - target_vel.linear_y = 0; + target_vel.linear_y = 0; } - + //convert m/s to m/min linear_vel_x_mins = target_vel.linear_x * 60; linear_vel_y_mins = target_vel.linear_y * 60; - + //convert rad/s to rad/min angular_vel_z_mins = target_vel.angular_z * 60; - + tangential_vel = angular_vel_z_mins * ((kin.length_x / 2) + (kin.length_y / 2)); x_rpm = linear_vel_x_mins / kin.wheel_cir; y_rpm = linear_vel_y_mins / kin.wheel_cir; tan_rpm = tangential_vel / kin.wheel_cir; - + // front-left motor cal_rpm.motor1 = x_rpm - y_rpm - tan_rpm; - + // front-right motor cal_rpm.motor2 = x_rpm + y_rpm + tan_rpm; - + // rear-left motor cal_rpm.motor3 = x_rpm + y_rpm - tan_rpm; - + // rear-right motor cal_rpm.motor4 = x_rpm - y_rpm + tan_rpm; if(kin.k_base == TWO_WD) { - res_rpm[0] = cal_rpm.motor3; - res_rpm[1] = cal_rpm.motor4; + res_rpm[0] = cal_rpm.motor3; + res_rpm[1] = cal_rpm.motor4; } else if(kin.k_base == FOUR_WD) { - res_rpm[0] = cal_rpm.motor1; - res_rpm[1] = cal_rpm.motor2; - res_rpm[2] = cal_rpm.motor3; - res_rpm[3] = cal_rpm.motor4; + res_rpm[0] = cal_rpm.motor1; + res_rpm[1] = cal_rpm.motor2; + res_rpm[2] = cal_rpm.motor3; + res_rpm[3] = cal_rpm.motor4; } else if(kin.k_base == ACKERMANN) { - res_rpm[0] = target_vel.angular_z; - res_rpm[1] = cal_rpm.motor3; - res_rpm[2] = cal_rpm.motor4; + res_rpm[0] = target_vel.angular_z; + res_rpm[1] = cal_rpm.motor3; + res_rpm[2] = cal_rpm.motor4; } else if(kin.k_base == MECANUM) { - res_rpm[0] = cal_rpm.motor1; - res_rpm[1] = cal_rpm.motor2; - res_rpm[2] = cal_rpm.motor3; - res_rpm[3] = cal_rpm.motor4; - } - else if(kin.k_base == FOUR_WD_ALLDIR) //FRONT:0 BACK:1 LEFT:2 RIGHT:3 - { - res_rpm[0] = x_rpm + tan_rpm; - res_rpm[1] = -x_rpm + tan_rpm; - res_rpm[2] = y_rpm + tan_rpm; - res_rpm[3] = -y_rpm + tan_rpm; - } + res_rpm[0] = cal_rpm.motor1; + res_rpm[1] = cal_rpm.motor2; + res_rpm[2] = cal_rpm.motor3; + res_rpm[3] = cal_rpm.motor4; + } + else if(kin.k_base == FOUR_WD_ALLDIR) //FRONT:0 BACK:1 LEFT:2 RIGHT:3 + { + res_rpm[0] = x_rpm + tan_rpm; + res_rpm[1] = -x_rpm + tan_rpm; + res_rpm[2] = y_rpm + tan_rpm; + res_rpm[3] = -y_rpm + tan_rpm; + } else { return; @@ -163,49 +163,49 @@ void kinematics_get_velocity(struct kinematics kin, struct rpm current_rpm, stru { // TODO struct velocity res_vel; - + int total_wheels = 0; if(kin.k_base == TWO_WD) total_wheels = 2; if(kin.k_base == FOUR_WD) total_wheels = 4; if(kin.k_base == ACKERMANN) total_wheels = 2; if(kin.k_base == MECANUM) total_wheels = 4; - if(kin.k_base == FOUR_WD_ALLDIR) total_wheels = 4; - + if(kin.k_base == FOUR_WD_ALLDIR) total_wheels = 4; + float average_rps_x; float average_rps_y; float average_rps_a; - - if(kin.k_base == TWO_WD || kin.k_base == FOUR_WD || kin.k_base == ACKERMANN) - { - //convert average revolutions per minute to revolutions per second + + if(kin.k_base == TWO_WD || kin.k_base == FOUR_WD || kin.k_base == ACKERMANN) + { + //convert average revolutions per minute to revolutions per second average_rps_x = ((float)(current_rpm.motor1 + current_rpm.motor2 + current_rpm.motor3 + current_rpm.motor4) / total_wheels) / 60; // RPM res_vel.linear_x = average_rps_x * kin.wheel_cir; // m/s - + //convert average revolutions per minute in y axis to revolutions per second average_rps_y = ((float)(-current_rpm.motor1 + current_rpm.motor2 + current_rpm.motor3 - current_rpm.motor4) / total_wheels) / 60; // RPM if(kin.k_base == MECANUM) res_vel.linear_y = average_rps_y * kin.wheel_cir; // m/s else res_vel.linear_y = 0; - + //convert average revolutions per minute to revolutions per second average_rps_a = ((float)(-current_rpm.motor1 + current_rpm.motor2 - current_rpm.motor3 + current_rpm.motor4) / total_wheels) / 60; res_vel.angular_z = (average_rps_a * kin.wheel_cir) / ((kin.length_x / 2) + (kin.length_y / 2)); // rad/s - } - if(kin.k_base == FOUR_WD_ALLDIR)//FRONT:motor1 BACK:motor2 LEFT:motor3 RIGHT:motor4 - { - //convert average revolutions per minute to revolutions per second - average_rps_x = ((float)(current_rpm.motor1 - current_rpm.motor2) / 2) / 60; // RPM - res_vel.linear_x = average_rps_x * kin.total_wheels; // m/s - - //convert average revolutions per minute in y axis to revolutions per second - average_rps_y = ((float)(current_rpm.motor3 - current_rpm.motor4) / 2) / 60; // RPM - res_vel.linear_y = average_rps_y * kin.wheel_cir; // m/s - - //convert average revolutions per minute to revolutions per second - average_rps_a = ((float)(current_rpm.motor1 + current_rpm.motor2 + current_rpm.motor3 + current_rpm.motor4 - - 2*60*(average_rps_x+average_rps_y)) / total_wheels) / 60; - res_vel.angular_z = (average_rps_a * kin.wheel_cir) / ((kin.length_x / 2) + (kin.length_y / 2)); // rad/s - } + } + if(kin.k_base == FOUR_WD_ALLDIR)//FRONT:motor1 BACK:motor2 LEFT:motor3 RIGHT:motor4 + { + //convert average revolutions per minute to revolutions per second + average_rps_x = ((float)(current_rpm.motor1 - current_rpm.motor2) / 2) / 60; // RPM + res_vel.linear_x = average_rps_x * kin.total_wheels; // m/s + + //convert average revolutions per minute in y axis to revolutions per second + average_rps_y = ((float)(current_rpm.motor3 - current_rpm.motor4) / 2) / 60; // RPM + res_vel.linear_y = average_rps_y * kin.wheel_cir; // m/s + + //convert average revolutions per minute to revolutions per second + average_rps_a = ((float)(current_rpm.motor1 + current_rpm.motor2 + current_rpm.motor3 + current_rpm.motor4 + - 2*60*(average_rps_x+average_rps_y)) / total_wheels) / 60; + res_vel.angular_z = (average_rps_a * kin.wheel_cir) / ((kin.length_x / 2) + (kin.length_y / 2)); // rad/s + } rt_memcpy(velocity, &res_vel, sizeof(struct velocity)); } diff --git a/kinematics/kinematics.h b/kinematics/kinematics.h index 116fa15..3965a79 100644 --- a/kinematics/kinematics.h +++ b/kinematics/kinematics.h @@ -18,7 +18,7 @@ enum base { FOUR_WD, ACKERMANN, MECANUM, - FOUR_WD_ALLDIR + FOUR_WD_ALLDIR }; // rad/min diff --git a/motor/can_motor.c b/motor/can_motor.c index 613d515..45783a4 100644 --- a/motor/can_motor.c +++ b/motor/can_motor.c @@ -9,22 +9,22 @@ static rt_err_t can_motor_enable(void *mot) { can_motor_t mot_sub = (can_motor_t)mot; - //Avoid repeated opening - if(!(mot_sub->can_dev->flag & RT_DEVICE_FLAG_ACTIVATED)) - { - rt_err_t res = rt_device_open(mot_sub->can_dev,RT_DEVICE_FLAG_INT_TX | RT_DEVICE_FLAG_INT_RX ); - res = rt_device_control(mot_sub->can_dev,RT_CAN_CMD_SET_BAUD,(void *)mot_sub->Baud); - res = rt_device_control(mot_sub->can_dev,RT_CAN_CMD_SET_FILTER,&mot_sub->cfg); - } - return RT_EOK; + //Avoid repeated opening + if(!(mot_sub->can_dev->flag & RT_DEVICE_FLAG_ACTIVATED)) + { + rt_err_t res = rt_device_open(mot_sub->can_dev,RT_DEVICE_FLAG_INT_TX | RT_DEVICE_FLAG_INT_RX ); + res = rt_device_control(mot_sub->can_dev,RT_CAN_CMD_SET_BAUD,(void *)mot_sub->Baud); + res = rt_device_control(mot_sub->can_dev,RT_CAN_CMD_SET_FILTER,&mot_sub->cfg); + } + return RT_EOK; } static rt_err_t can_motor_disable(void *mot) { can_motor_t mot_sub = (can_motor_t)mot; - + rt_err_t res = rt_device_close(mot_sub->can_dev); - + return RT_EOK; } @@ -36,52 +36,52 @@ static rt_err_t can_motor_reset(void *mot) static rt_err_t can_motor_set_speed(void *mot, rt_int16_t thousands) { can_motor_t mot_sub = (can_motor_t)mot; - - if(mot_sub->min_num > thousands) - { - thousands = mot_sub->min_num; - } - else if(mot_sub->max_num < thousands) - { - thousands = mot_sub->max_num; - } - - mot_sub->msg->data[(mot_sub->mot_id -1)*2] = thousands>>8; - mot_sub->msg->data[(mot_sub->mot_id -1)*2+1] = thousands; - rt_size_t size = rt_device_write(mot_sub->can_dev, 0, mot_sub->msg, sizeof(*(mot_sub->msg)) ); - + + if(mot_sub->min_num > thousands) + { + thousands = mot_sub->min_num; + } + else if(mot_sub->max_num < thousands) + { + thousands = mot_sub->max_num; + } + + mot_sub->msg->data[(mot_sub->mot_id -1)*2] = thousands>>8; + mot_sub->msg->data[(mot_sub->mot_id -1)*2+1] = thousands; + rt_size_t size = rt_device_write(mot_sub->can_dev, 0, mot_sub->msg, sizeof(*(mot_sub->msg)) ); + return RT_EOK; } can_motor_t can_motor_create(char *can,struct rt_can_filter_config cfg,rt_uint32_t Baud,rt_can_msg_t msg,rt_int16_t max_num,rt_int16_t min_num,rt_uint8_t mot_id) { - RT_ASSERT(max_num>min_num) - + RT_ASSERT(max_num>min_num) + can_motor_t new_motor = (can_motor_t)motor_create(sizeof(struct can_motor)); if (new_motor == RT_NULL) { return RT_NULL; } - + new_motor->can_dev = (rt_device_t)rt_device_find(can); if (new_motor->can_dev == RT_NULL) { - rt_free(new_motor); - LOG_E("Falied to find device on %s", can); - return RT_NULL; + rt_free(new_motor); + LOG_E("Falied to find device on %s", can); + return RT_NULL; } - - new_motor->cfg = cfg; - new_motor->Baud = Baud; - new_motor->msg = msg; - new_motor->max_num = max_num; - new_motor->min_num = min_num; - new_motor->mot_id = mot_id; - new_motor->mot.enable = can_motor_enable; - new_motor->mot.set_speed = can_motor_set_speed; - new_motor->mot.reset = can_motor_reset; - new_motor->mot.disable = can_motor_disable; - + + new_motor->cfg = cfg; + new_motor->Baud = Baud; + new_motor->msg = msg; + new_motor->max_num = max_num; + new_motor->min_num = min_num; + new_motor->mot_id = mot_id; + new_motor->mot.enable = can_motor_enable; + new_motor->mot.set_speed = can_motor_set_speed; + new_motor->mot.reset = can_motor_reset; + new_motor->mot.disable = can_motor_disable; + return new_motor; } diff --git a/motor/can_motor.h b/motor/can_motor.h index 5741a23..5293b99 100644 --- a/motor/can_motor.h +++ b/motor/can_motor.h @@ -5,14 +5,14 @@ struct can_motor { - struct motor mot; - rt_device_t can_dev; + struct motor mot; + rt_device_t can_dev; struct rt_can_filter_config cfg; - rt_uint32_t Baud; - rt_can_msg_t msg; //the pointer of message defined by the users,it decide the number of the can motor in the can net - rt_int16_t max_num; - rt_int16_t min_num; - rt_uint8_t mot_id; //the id of the can motor in the can net + rt_uint32_t Baud; + rt_can_msg_t msg; //the pointer of message defined by the users,it decide the number of the can motor in the can net + rt_int16_t max_num; + rt_int16_t min_num; + rt_uint8_t mot_id; //the id of the can motor in the can net }; typedef struct can_motor *can_motor_t; From 54112bc797789196b739611d993b678f09e48d02 Mon Sep 17 00:00:00 2001 From: Soil_L Date: Sat, 18 Jan 2020 21:56:46 +0800 Subject: [PATCH 3/5] doc:add the copyright notice --- encoder/can_encoder.c | 9 +++++++++ encoder/can_encoder.h | 9 +++++++++ encoder/encoder.c | 1 + kinematics/kinematics.c | 1 + kinematics/kinematics.h | 1 + motor/can_motor.c | 9 +++++++++ motor/can_motor.h | 9 +++++++++ 7 files changed, 39 insertions(+) diff --git a/encoder/can_encoder.c b/encoder/can_encoder.c index d6351fb..f1ee2b3 100644 --- a/encoder/can_encoder.c +++ b/encoder/can_encoder.c @@ -1,3 +1,12 @@ +/* + * Copyright (c) 2019, RT-Thread Development Team + * + * SPDX-License-Identifier: Apache-2.0 + * + * Change Logs: + * Date Author Notes + * 2019-11-14 Soil_L The first version + */ #include "can_encoder.h" #define DBG_SECTION_NAME "can_encoder" diff --git a/encoder/can_encoder.h b/encoder/can_encoder.h index 0323a8d..ca5aa1f 100644 --- a/encoder/can_encoder.h +++ b/encoder/can_encoder.h @@ -1,3 +1,12 @@ +/* + * Copyright (c) 2019, RT-Thread Development Team + * + * SPDX-License-Identifier: Apache-2.0 + * + * Change Logs: + * Date Author Notes + * 2019-11-14 Soil_L The first version + */ #ifndef _CAN_ENCODER_ #define _CAN_ENCODER_ diff --git a/encoder/encoder.c b/encoder/encoder.c index 262889d..03efa56 100644 --- a/encoder/encoder.c +++ b/encoder/encoder.c @@ -6,6 +6,7 @@ * Change Logs: * Date Author Notes * 2019-08-26 sogwms The first version + * 2019-11-14 Soil_L motified for the need of can encoder */ #include diff --git a/kinematics/kinematics.c b/kinematics/kinematics.c index 9a49ca8..2beaaae 100644 --- a/kinematics/kinematics.c +++ b/kinematics/kinematics.c @@ -6,6 +6,7 @@ * Change Logs: * Date Author Notes * 2019-07-17 Wu Han The first version + * 2019-11-14 Soil_L add four wheel omnidirectional chassis */ #include "kinematics.h" diff --git a/kinematics/kinematics.h b/kinematics/kinematics.h index 3965a79..986f221 100644 --- a/kinematics/kinematics.h +++ b/kinematics/kinematics.h @@ -6,6 +6,7 @@ * Change Logs: * Date Author Notes * 2019-07-17 Wu Han The first version + * 2019-11-14 Soil_L add four wheel omnidirectional */ #ifndef __KINEMATICS_H__ diff --git a/motor/can_motor.c b/motor/can_motor.c index 45783a4..490cecb 100644 --- a/motor/can_motor.c +++ b/motor/can_motor.c @@ -1,3 +1,12 @@ +/* + * Copyright (c) 2019, RT-Thread Development Team + * + * SPDX-License-Identifier: Apache-2.0 + * + * Change Logs: + * Date Author Notes + * 2019-11-14 Soil_L The first version + */ #include "can_motor.h" #define DBG_SECTION_NAME "can_motor" diff --git a/motor/can_motor.h b/motor/can_motor.h index 5293b99..62b8cf0 100644 --- a/motor/can_motor.h +++ b/motor/can_motor.h @@ -1,3 +1,12 @@ +/* + * Copyright (c) 2019, RT-Thread Development Team + * + * SPDX-License-Identifier: Apache-2.0 + * + * Change Logs: + * Date Author Notes + * 2019-11-14 Soil_L The first version + */ #ifndef _CAN_MOTOR_H_ #define _CAN_MOTOR_H_ From fc8e02099295b8d63bcc290df7eb561f5e46c5ff Mon Sep 17 00:00:00 2001 From: Soil_L Date: Sat, 18 Jan 2020 22:14:43 +0800 Subject: [PATCH 4/5] refactor:align according to specifications --- encoder/can_encoder.c | 14 ++++---- encoder/encoder.c | 32 +++++++++--------- kinematics/kinematics.c | 72 ++++++++++++++++++++--------------------- motor/can_motor.c | 16 ++++----- motor/can_motor.h | 12 +++---- 5 files changed, 73 insertions(+), 73 deletions(-) diff --git a/encoder/can_encoder.c b/encoder/can_encoder.c index f1ee2b3..fa86e2f 100644 --- a/encoder/can_encoder.c +++ b/encoder/can_encoder.c @@ -30,9 +30,9 @@ static rt_err_t can_encoder_enable(void *enc) //Avoid repeated opening if(!(enc_sub->can_dev->flag & RT_DEVICE_FLAG_ACTIVATED)) { - res = rt_device_open(enc_sub->can_dev,RT_DEVICE_FLAG_INT_RX|RT_DEVICE_FLAG_INT_TX); - res = rt_device_control(enc_sub->can_dev,RT_CAN_CMD_SET_BAUD,(void *)enc_sub->Baud); - res = rt_device_control(enc_sub->can_dev,RT_CAN_CMD_SET_FILTER,&enc_sub->cfg); + res = rt_device_open(enc_sub->can_dev,RT_DEVICE_FLAG_INT_RX|RT_DEVICE_FLAG_INT_TX); + res = rt_device_control(enc_sub->can_dev,RT_CAN_CMD_SET_BAUD,(void *)enc_sub->Baud); + res = rt_device_control(enc_sub->can_dev,RT_CAN_CMD_SET_FILTER,&enc_sub->cfg); } res = rt_device_set_rx_indicate(enc_sub->can_dev,encoder_isr); @@ -61,15 +61,15 @@ can_encoder_t can_encoder_create(char *can,struct rt_can_filter_config cfg,rt_ui can_encoder_t new_encoder = (can_encoder_t)encoder_create(sizeof(struct can_encoder),0); if(new_encoder == RT_NULL) { - return RT_NULL; + return RT_NULL; } new_encoder->can_dev = (rt_device_t)rt_device_find(can); if (new_encoder->can_dev == RT_NULL) { - rt_free(new_encoder); - LOG_E("Falied to find device on %s", can); - return RT_NULL; + rt_free(new_encoder); + LOG_E("Falied to find device on %s", can); + return RT_NULL; } new_encoder->cfg = cfg; diff --git a/encoder/encoder.c b/encoder/encoder.c index 03efa56..db1b064 100644 --- a/encoder/encoder.c +++ b/encoder/encoder.c @@ -22,14 +22,14 @@ encoder_t encoder_create(rt_size_t size, rt_uint16_t sample_time) encoder_t new_encoder = (encoder_t)rt_malloc(size); if(new_encoder == RT_NULL) { - LOG_E("Failed to malloc memory for new encoder\n"); - return RT_NULL; + LOG_E("Failed to malloc memory for new encoder\n"); + return RT_NULL; } - + new_encoder->pulse_count = 0; new_encoder->last_count = 0; new_encoder->sample_time = sample_time; - + return new_encoder; } @@ -37,7 +37,7 @@ rt_err_t encoder_destroy(encoder_t enc) { LOG_D("Free Encoder"); RT_ASSERT(enc != RT_NULL); - + return enc->destroy(enc); } @@ -45,7 +45,7 @@ rt_err_t encoder_enable(encoder_t enc) { LOG_D("Enabling encoder"); RT_ASSERT(enc != RT_NULL); - + return enc->enable(enc); } @@ -53,44 +53,44 @@ rt_err_t encoder_disable(encoder_t enc) { LOG_D("Diabling encoder"); RT_ASSERT(enc != RT_NULL); - + return enc->disable(enc); } rt_int32_t encoder_read(encoder_t enc) { RT_ASSERT(enc != RT_NULL); - + return enc->pulse_count; } rt_err_t encoder_reset(encoder_t enc) { RT_ASSERT(enc != RT_NULL); - + enc->pulse_count = 0; enc->last_count = 0; - + return RT_EOK; } rt_int16_t encoder_measure_cps(encoder_t enc) { RT_ASSERT(enc != RT_NULL); - + // return count per second if((rt_tick_get() - enc->last_time) < rt_tick_from_millisecond(enc->sample_time)) { - LOG_D("Encoder waiting ... "); - return enc->cps; + LOG_D("Encoder waiting ... "); + return enc->cps; } - + rt_int32_t diff_count = enc->pulse_count - enc->last_count; - + enc->cps = diff_count * 1000 / enc->sample_time; enc->last_count = enc->pulse_count; enc->last_time = rt_tick_get(); - + return enc->cps; } diff --git a/kinematics/kinematics.c b/kinematics/kinematics.c index 2beaaae..63be170 100644 --- a/kinematics/kinematics.c +++ b/kinematics/kinematics.c @@ -22,10 +22,10 @@ kinematics_t kinematics_create(enum base k_base, float length_x, float length_y, kinematics_t new_kinematics = (kinematics_t)rt_malloc(sizeof(struct kinematics)); if(new_kinematics == RT_NULL) { - LOG_E("Failed to malloc memory for kinematics\n"); - return RT_NULL; + LOG_E("Failed to malloc memory for kinematics\n"); + return RT_NULL; } - + new_kinematics->k_base = k_base; new_kinematics->length_x = length_x; new_kinematics->length_y = length_y; @@ -33,23 +33,23 @@ kinematics_t kinematics_create(enum base k_base, float length_x, float length_y, if(k_base == TWO_WD) { - new_kinematics->total_wheels = 2; + new_kinematics->total_wheels = 2; } if(k_base == FOUR_WD) { - new_kinematics->total_wheels = 4; + new_kinematics->total_wheels = 4; } if(k_base == ACKERMANN) { - new_kinematics->total_wheels = 3; + new_kinematics->total_wheels = 3; } if(k_base == MECANUM) { - new_kinematics->total_wheels = 4; + new_kinematics->total_wheels = 4; } if(k_base == FOUR_WD_ALLDIR) { - new_kinematics->total_wheels = 4; + new_kinematics->total_wheels = 4; } return new_kinematics; @@ -76,21 +76,21 @@ void kinematics_get_rpm(struct kinematics kin, struct velocity target_vel, rt_in // TODO struct rpm cal_rpm; rt_int16_t res_rpm[4] = {0}; - + float linear_vel_x_mins; float linear_vel_y_mins; float angular_vel_z_mins; float tangential_vel; - + float x_rpm; float y_rpm; float tan_rpm; if(kin.k_base == TWO_WD || kin.k_base == FOUR_WD || kin.k_base == ACKERMANN) { - target_vel.linear_y = 0; + target_vel.linear_y = 0; } - + //convert m/s to m/min linear_vel_x_mins = target_vel.linear_x * 60; linear_vel_y_mins = target_vel.linear_y * 60; @@ -103,50 +103,50 @@ void kinematics_get_rpm(struct kinematics kin, struct velocity target_vel, rt_in x_rpm = linear_vel_x_mins / kin.wheel_cir; y_rpm = linear_vel_y_mins / kin.wheel_cir; tan_rpm = tangential_vel / kin.wheel_cir; - + // front-left motor cal_rpm.motor1 = x_rpm - y_rpm - tan_rpm; - + // front-right motor cal_rpm.motor2 = x_rpm + y_rpm + tan_rpm; - + // rear-left motor cal_rpm.motor3 = x_rpm + y_rpm - tan_rpm; - + // rear-right motor cal_rpm.motor4 = x_rpm - y_rpm + tan_rpm; if(kin.k_base == TWO_WD) { - res_rpm[0] = cal_rpm.motor3; - res_rpm[1] = cal_rpm.motor4; + res_rpm[0] = cal_rpm.motor3; + res_rpm[1] = cal_rpm.motor4; } else if(kin.k_base == FOUR_WD) { - res_rpm[0] = cal_rpm.motor1; - res_rpm[1] = cal_rpm.motor2; - res_rpm[2] = cal_rpm.motor3; - res_rpm[3] = cal_rpm.motor4; + res_rpm[0] = cal_rpm.motor1; + res_rpm[1] = cal_rpm.motor2; + res_rpm[2] = cal_rpm.motor3; + res_rpm[3] = cal_rpm.motor4; } else if(kin.k_base == ACKERMANN) { - res_rpm[0] = target_vel.angular_z; - res_rpm[1] = cal_rpm.motor3; - res_rpm[2] = cal_rpm.motor4; + res_rpm[0] = target_vel.angular_z; + res_rpm[1] = cal_rpm.motor3; + res_rpm[2] = cal_rpm.motor4; } else if(kin.k_base == MECANUM) { - res_rpm[0] = cal_rpm.motor1; - res_rpm[1] = cal_rpm.motor2; - res_rpm[2] = cal_rpm.motor3; - res_rpm[3] = cal_rpm.motor4; + res_rpm[0] = cal_rpm.motor1; + res_rpm[1] = cal_rpm.motor2; + res_rpm[2] = cal_rpm.motor3; + res_rpm[3] = cal_rpm.motor4; } else if(kin.k_base == FOUR_WD_ALLDIR) //FRONT:0 BACK:1 LEFT:2 RIGHT:3 { - res_rpm[0] = x_rpm + tan_rpm; - res_rpm[1] = -x_rpm + tan_rpm; - res_rpm[2] = y_rpm + tan_rpm; - res_rpm[3] = -y_rpm + tan_rpm; + res_rpm[0] = x_rpm + tan_rpm; + res_rpm[1] = -x_rpm + tan_rpm; + res_rpm[2] = y_rpm + tan_rpm; + res_rpm[3] = -y_rpm + tan_rpm; } else { @@ -164,18 +164,18 @@ void kinematics_get_velocity(struct kinematics kin, struct rpm current_rpm, stru { // TODO struct velocity res_vel; - + int total_wheels = 0; if(kin.k_base == TWO_WD) total_wheels = 2; if(kin.k_base == FOUR_WD) total_wheels = 4; if(kin.k_base == ACKERMANN) total_wheels = 2; if(kin.k_base == MECANUM) total_wheels = 4; if(kin.k_base == FOUR_WD_ALLDIR) total_wheels = 4; - + float average_rps_x; float average_rps_y; float average_rps_a; - + if(kin.k_base == TWO_WD || kin.k_base == FOUR_WD || kin.k_base == ACKERMANN) { //convert average revolutions per minute to revolutions per second diff --git a/motor/can_motor.c b/motor/can_motor.c index 490cecb..98bf19c 100644 --- a/motor/can_motor.c +++ b/motor/can_motor.c @@ -21,9 +21,9 @@ static rt_err_t can_motor_enable(void *mot) //Avoid repeated opening if(!(mot_sub->can_dev->flag & RT_DEVICE_FLAG_ACTIVATED)) { - rt_err_t res = rt_device_open(mot_sub->can_dev,RT_DEVICE_FLAG_INT_TX | RT_DEVICE_FLAG_INT_RX ); - res = rt_device_control(mot_sub->can_dev,RT_CAN_CMD_SET_BAUD,(void *)mot_sub->Baud); - res = rt_device_control(mot_sub->can_dev,RT_CAN_CMD_SET_FILTER,&mot_sub->cfg); + rt_err_t res = rt_device_open(mot_sub->can_dev,RT_DEVICE_FLAG_INT_TX | RT_DEVICE_FLAG_INT_RX ); + res = rt_device_control(mot_sub->can_dev,RT_CAN_CMD_SET_BAUD,(void *)mot_sub->Baud); + res = rt_device_control(mot_sub->can_dev,RT_CAN_CMD_SET_FILTER,&mot_sub->cfg); } return RT_EOK; } @@ -48,11 +48,11 @@ static rt_err_t can_motor_set_speed(void *mot, rt_int16_t thousands) if(mot_sub->min_num > thousands) { - thousands = mot_sub->min_num; + thousands = mot_sub->min_num; } else if(mot_sub->max_num < thousands) { - thousands = mot_sub->max_num; + thousands = mot_sub->max_num; } mot_sub->msg->data[(mot_sub->mot_id -1)*2] = thousands>>8; @@ -76,9 +76,9 @@ can_motor_t can_motor_create(char *can,struct rt_can_filter_config cfg,rt_uint32 new_motor->can_dev = (rt_device_t)rt_device_find(can); if (new_motor->can_dev == RT_NULL) { - rt_free(new_motor); - LOG_E("Falied to find device on %s", can); - return RT_NULL; + rt_free(new_motor); + LOG_E("Falied to find device on %s", can); + return RT_NULL; } new_motor->cfg = cfg; diff --git a/motor/can_motor.h b/motor/can_motor.h index 62b8cf0..c4587db 100644 --- a/motor/can_motor.h +++ b/motor/can_motor.h @@ -16,12 +16,12 @@ struct can_motor { struct motor mot; rt_device_t can_dev; - struct rt_can_filter_config cfg; - rt_uint32_t Baud; - rt_can_msg_t msg; //the pointer of message defined by the users,it decide the number of the can motor in the can net - rt_int16_t max_num; - rt_int16_t min_num; - rt_uint8_t mot_id; //the id of the can motor in the can net + struct rt_can_filter_config cfg; + rt_uint32_t Baud; + rt_can_msg_t msg; //the pointer of message defined by the users,it decide the number of the can motor in the can net + rt_int16_t max_num; + rt_int16_t min_num; + rt_uint8_t mot_id; //the id of the can motor in the can net }; typedef struct can_motor *can_motor_t; From b1478c5f92518adc2949d75b56d6b4f46d91c6c5 Mon Sep 17 00:00:00 2001 From: Soil_L Date: Sat, 18 Jan 2020 22:19:39 +0800 Subject: [PATCH 5/5] refactor:delete some tabs --- encoder/encoder.c | 2 +- kinematics/kinematics.c | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/encoder/encoder.c b/encoder/encoder.c index db1b064..f3a85ce 100644 --- a/encoder/encoder.c +++ b/encoder/encoder.c @@ -77,7 +77,7 @@ rt_err_t encoder_reset(encoder_t enc) rt_int16_t encoder_measure_cps(encoder_t enc) { RT_ASSERT(enc != RT_NULL); - + // return count per second if((rt_tick_get() - enc->last_time) < rt_tick_from_millisecond(enc->sample_time)) { diff --git a/kinematics/kinematics.c b/kinematics/kinematics.c index 63be170..3ed2c5d 100644 --- a/kinematics/kinematics.c +++ b/kinematics/kinematics.c @@ -30,7 +30,7 @@ kinematics_t kinematics_create(enum base k_base, float length_x, float length_y, new_kinematics->length_x = length_x; new_kinematics->length_y = length_y; new_kinematics->wheel_cir = wheel_radius * 2.0f * PI;; - + if(k_base == TWO_WD) { new_kinematics->total_wheels = 2; @@ -94,10 +94,10 @@ void kinematics_get_rpm(struct kinematics kin, struct velocity target_vel, rt_in //convert m/s to m/min linear_vel_x_mins = target_vel.linear_x * 60; linear_vel_y_mins = target_vel.linear_y * 60; - + //convert rad/s to rad/min angular_vel_z_mins = target_vel.angular_z * 60; - + tangential_vel = angular_vel_z_mins * ((kin.length_x / 2) + (kin.length_y / 2)); x_rpm = linear_vel_x_mins / kin.wheel_cir;