diff --git a/encoder/can_encoder.c b/encoder/can_encoder.c new file mode 100644 index 0000000..fa86e2f --- /dev/null +++ b/encoder/can_encoder.c @@ -0,0 +1,83 @@ +/* + * 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" +#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..ca5aa1f --- /dev/null +++ b/encoder/can_encoder.h @@ -0,0 +1,28 @@ +/* + * 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_ + +#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..f3a85ce 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 @@ -96,10 +97,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 3ceb7d4..3ed2c5d 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" @@ -46,7 +47,11 @@ 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; } @@ -136,6 +141,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; @@ -158,25 +170,43 @@ 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_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 == 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 + } rt_memcpy(velocity, &res_vel, sizeof(struct velocity)); } diff --git a/kinematics/kinematics.h b/kinematics/kinematics.h index c356514..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__ @@ -17,7 +18,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..98bf19c --- /dev/null +++ b/motor/can_motor.c @@ -0,0 +1,96 @@ +/* + * 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" +#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..c4587db --- /dev/null +++ b/motor/can_motor.h @@ -0,0 +1,30 @@ +/* + * 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_ + +#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__