Wheel control software for satellite microcontroller running the motors on the Karbor
Dependencies: mbed ros_lib_melodic
Diff: src/main.cpp
- Revision:
- 6:ed47deb76adf
- Parent:
- 5:44b2454a5eea
diff -r 44b2454a5eea -r ed47deb76adf src/main.cpp --- a/src/main.cpp Tue Jun 08 15:04:33 2021 +0000 +++ b/src/main.cpp Tue Jun 08 20:55:08 2021 +0000 @@ -1,4 +1,6 @@ /** @file main.cpp + * @author Simon Krogedal + * @version 0.1 */ /* Karbot motor controller @@ -32,10 +34,18 @@ #define R_MOTOR_DIR PB_10 #define L_MOTOR_DIR PA_9 -#define R_ENC_PIN_A PB_5 -#define R_ENC_PIN_B PB_4 +#define R_ENC_PIN_A PB_5 // Blue +#define R_ENC_PIN_B PB_4 // Purple #define L_ENC_PIN_A PB_3 #define L_ENC_PIN_B PC_13 + +/* These pin definitions were used in ESP, it might be smart to go use those +#define L_ENC_PIN_A PA_14 +#define L_ENC_PIN_B PB_15 +#define R_ENC_PIN_A PA_13 +#define R_ENC_PIN_B PB_1 +*/ + #define BAT_A_PIN PA_0 #define BAT_B_PIN PA_1 @@ -45,7 +55,7 @@ //VARIABLES -#define MOTOR_FREQ 25000.0 // 25 kHz, freq of motor PWM +#define MOTOR_FREQ 20000.0 // 20 kHz, freq of motor PWM, max for driver board #define MOTOR_TS 0.05 // Sample time, ie the time between every PID calculation #define MOTOR_KP 0.00002 // Loop motor gain #define MOTOR_TI 200 // Integral motor gain @@ -75,8 +85,8 @@ motor leftMot (L_MOTOR_PIN, L_MOTOR_DIR, 1/MOTOR_FREQ); motor rightMot (R_MOTOR_PIN, R_MOTOR_DIR, 1/MOTOR_FREQ); -MotorControl leftCtrl (L_ENC_PIN_A, L_ENC_PIN_B, ENCODER_CPR, 1, MOTOR_TS, &leftMot, L_MAX_SPEED, MOTOR_KP, MOTOR_TI, WHEEL_RAD); -MotorControl rightCtrl (R_ENC_PIN_A, R_ENC_PIN_B, ENCODER_CPR, 0, MOTOR_TS, &rightMot, R_MAX_SPEED, MOTOR_KP, MOTOR_TI, WHEEL_RAD); +MotorControl leftCtrl (L_ENC_PIN_A, L_ENC_PIN_B, ENCODER_CPR, encoder::Left, MOTOR_TS, &leftMot, L_MAX_SPEED, MOTOR_KP, MOTOR_TI, WHEEL_RAD); +MotorControl rightCtrl (R_ENC_PIN_A, R_ENC_PIN_B, ENCODER_CPR, encoder::Right, MOTOR_TS, &rightMot, R_MAX_SPEED, MOTOR_KP, MOTOR_TI, WHEEL_RAD); // Callback function for a twist message given by the motion controller void messageCb(const geometry_msgs::Twist& msg); @@ -107,9 +117,12 @@ // Set up joint state message - //wheels_msg.name = ["Left wheel","Right wheel"]; - wheels_msg.name[0] = "Left wheel"; - wheels_msg.name[1] = "Right wheel"; + wheels_msg.name_length = 2; // In these libs, lenght must be set + wheels_msg.velocity_length = 2; // manually for each term + + // Name joints + char *jointNames[] = {"Left wheel","Right wheel"}; + wheels_msg.name = jointNames; // Set up battery messages batA_msg.power_supply_technology = 2; @@ -180,8 +193,8 @@ nh.advertise(right_wheel_pub); nh.loginfo("Setup complete"); - leftCtrl.start(); - rightCtrl.start(); + //leftCtrl.start(); + //rightCtrl.start(); // Main loop while(true) { @@ -209,8 +222,8 @@ batB_pub.publish(&batB_msg); wheels_pub.publish(&wheels_msg); - left_wheel_pub.publish(@left_wheel_speed_msg); - right_wheel_pub.publish(@right_wheel_speed_msg); + left_wheel_pub.publish(&left_wheel_speed_msg); + right_wheel_pub.publish(&right_wheel_speed_msg); nh.spinOnce();