Thomas Godden / Mbed 2 deprecated eurobot-2020_motor-breakout

mbed code for the nucleo powered motor driver and encoder breakout from ICRS's Eurobot 2020-2021 robot. Interfaces with rosserial.

Dependencies:   mbed QEI ros_lib_melodic millis

main.cpp@2:65e922b3569d, 2021-09-15 (annotated)

Committer:
thomasgg
Date:
Wed Sep 15 16:07:21 2021 +0000
Revision:
2:65e922b3569d
Parent:
1:ac92cf476127 
Committed to current version

Who changed what in which revision?

UserRevisionLine numberNew contents of line
thomasgg 0:f0117a02cd54 1 #include "mbed.h"
thomasgg 0:f0117a02cd54 2 #include "QEI.h"
thomasgg 0:f0117a02cd54 3 #include <ros.h>
thomasgg 0:f0117a02cd54 4 #include <std_msgs/String.h>
thomasgg 0:f0117a02cd54 5 #include <std_msgs/Float32.h>
thomasgg 2:65e922b3569d 6
thomasgg 2:65e922b3569d 7 #include "millis.h"
thomasgg 0:f0117a02cd54 8
thomasgg 0:f0117a02cd54 9 // ===== ENCODERS
thomasgg 1:ac92cf476127 10 // motor_pulses_per_rev
thomasgg 1:ac92cf476127 11 int pulses_per_rev = (2000); // motor revs
thomasgg 1:ac92cf476127 12
thomasgg 1:ac92cf476127 13 // Gear ratio / (2 * pi) * encoder_counts
thomasgg 1:ac92cf476127 14 float pulses_per_wheel_rad = (35 / 28) * (2000) / (2 * 3.14159);
thomasgg 0:f0117a02cd54 15
thomasgg 2:65e922b3569d 16 // Prev encoder value
thomasgg 2:65e922b3569d 17 float wheel0 = 0;
thomasgg 2:65e922b3569d 18 float wheel1 = 0;
thomasgg 2:65e922b3569d 19 float wheel2 = 0;
thomasgg 2:65e922b3569d 20 float wheel3 = 0;
thomasgg 2:65e922b3569d 21
thomasgg 2:65e922b3569d 22 float wheel0_prev = 0;
thomasgg 2:65e922b3569d 23 float wheel1_prev = 0;
thomasgg 2:65e922b3569d 24 float wheel2_prev = 0;
thomasgg 2:65e922b3569d 25 float wheel3_prev = 0;
thomasgg 2:65e922b3569d 26
thomasgg 2:65e922b3569d 27 float kP = 0.05;
thomasgg 2:65e922b3569d 28 float kF = 0.035;
thomasgg 2:65e922b3569d 29
thomasgg 2:65e922b3569d 30 int pub_skip = 2;
thomasgg 2:65e922b3569d 31 int count = 0;
thomasgg 2:65e922b3569d 32
thomasgg 2:65e922b3569d 33 float dt = 0.005;
thomasgg 2:65e922b3569d 34 float prev_t = 0;
thomasgg 2:65e922b3569d 35
thomasgg 2:65e922b3569d 36 // Motor speed set points
thomasgg 2:65e922b3569d 37 float motor0_sp = 0;
thomasgg 2:65e922b3569d 38 float motor1_sp = 0;
thomasgg 2:65e922b3569d 39 float motor2_sp = 0;
thomasgg 2:65e922b3569d 40 float motor3_sp = 0;
thomasgg 2:65e922b3569d 41
thomasgg 0:f0117a02cd54 42 // Verify encoder lengths
thomasgg 0:f0117a02cd54 43 QEI encP0(A0, A2, NC, pulses_per_rev);
thomasgg 1:ac92cf476127 44 QEI encP1(D6, D12, NC, pulses_per_rev);
thomasgg 0:f0117a02cd54 45 QEI encP2(A3, A6, NC, pulses_per_rev);
thomasgg 0:f0117a02cd54 46 QEI encP3(D4, D5, NC, pulses_per_rev);
thomasgg 0:f0117a02cd54 47
thomasgg 1:ac92cf476127 48 // ===== MOTOR outputs
thomasgg 1:ac92cf476127 49 DigitalOut motor2A(D13);
thomasgg 1:ac92cf476127 50 DigitalOut motor2B(A1);
thomasgg 1:ac92cf476127 51
thomasgg 1:ac92cf476127 52 DigitalOut motor1A(D8);
thomasgg 1:ac92cf476127 53 DigitalOut motor1B(D10);
thomasgg 1:ac92cf476127 54
thomasgg 1:ac92cf476127 55
thomasgg 1:ac92cf476127 56 DigitalOut motor3A(A4);
thomasgg 1:ac92cf476127 57 DigitalOut motor3B(A5);
thomasgg 1:ac92cf476127 58
thomasgg 1:ac92cf476127 59 DigitalOut motor0A(D2);
thomasgg 1:ac92cf476127 60 DigitalOut motor0B(D3);
thomasgg 1:ac92cf476127 61
thomasgg 1:ac92cf476127 62 PwmOut motor2_En(D0);
thomasgg 1:ac92cf476127 63 PwmOut motor1_En(D9);
thomasgg 1:ac92cf476127 64 PwmOut motor3_En(D1);
thomasgg 1:ac92cf476127 65 PwmOut motor0_En(D11);
thomasgg 1:ac92cf476127 66
thomasgg 1:ac92cf476127 67
thomasgg 2:65e922b3569d 68 void setMotor0Power(float power){
thomasgg 1:ac92cf476127 69 if( power > 0 ){
thomasgg 1:ac92cf476127 70 motor0A = 1;
thomasgg 1:ac92cf476127 71 motor0B = 0;
thomasgg 1:ac92cf476127 72 }else if( power < 0 ){
thomasgg 1:ac92cf476127 73 motor0A = 0;
thomasgg 1:ac92cf476127 74 motor0B = 1;
thomasgg 1:ac92cf476127 75 } else {
thomasgg 1:ac92cf476127 76 motor0A = 0;
thomasgg 1:ac92cf476127 77 motor0B = 0;
thomasgg 1:ac92cf476127 78 }
thomasgg 1:ac92cf476127 79 motor0_En = abs(power);
thomasgg 1:ac92cf476127 80 }
thomasgg 1:ac92cf476127 81
thomasgg 2:65e922b3569d 82 void setMotor1Power(float power){
thomasgg 1:ac92cf476127 83 if( power > 0 ){
thomasgg 1:ac92cf476127 84 motor1A = 1;
thomasgg 1:ac92cf476127 85 motor1B = 0;
thomasgg 1:ac92cf476127 86 }else if( power < 0 ){
thomasgg 1:ac92cf476127 87 motor1A = 0;
thomasgg 1:ac92cf476127 88 motor1B = 1;
thomasgg 1:ac92cf476127 89 } else {
thomasgg 1:ac92cf476127 90 motor1A = 0;
thomasgg 1:ac92cf476127 91 motor1B = 0;
thomasgg 1:ac92cf476127 92 }
thomasgg 1:ac92cf476127 93 motor1_En = abs(power);
thomasgg 1:ac92cf476127 94 }
thomasgg 1:ac92cf476127 95
thomasgg 2:65e922b3569d 96 void setMotor2Power(float power){
thomasgg 1:ac92cf476127 97 if( power > 0 ){
thomasgg 1:ac92cf476127 98 motor2A = 1;
thomasgg 1:ac92cf476127 99 motor2B = 0;
thomasgg 2:65e922b3569d 100 }else if(power < 0 ){
thomasgg 1:ac92cf476127 101 motor2A = 0;
thomasgg 1:ac92cf476127 102 motor2B = 1;
thomasgg 1:ac92cf476127 103 } else {
thomasgg 1:ac92cf476127 104 motor2A = 0;
thomasgg 1:ac92cf476127 105 motor2B = 0;
thomasgg 1:ac92cf476127 106 }
thomasgg 1:ac92cf476127 107 motor2_En = abs(power);
thomasgg 1:ac92cf476127 108 }
thomasgg 1:ac92cf476127 109
thomasgg 2:65e922b3569d 110 void setMotor3Power(float power){
thomasgg 1:ac92cf476127 111 if( power > 0 ){
thomasgg 1:ac92cf476127 112 motor3A = 1;
thomasgg 1:ac92cf476127 113 motor3B = 0;
thomasgg 1:ac92cf476127 114 }else if( power < 0 ){
thomasgg 1:ac92cf476127 115 motor3A = 0;
thomasgg 1:ac92cf476127 116 motor3B = 1;
thomasgg 1:ac92cf476127 117 } else {
thomasgg 1:ac92cf476127 118 motor3A = 0;
thomasgg 1:ac92cf476127 119 motor3B = 0;
thomasgg 1:ac92cf476127 120 }
thomasgg 1:ac92cf476127 121 motor3_En = abs(power);
thomasgg 1:ac92cf476127 122 }
thomasgg 1:ac92cf476127 123
thomasgg 2:65e922b3569d 124 // ======= MOTOR CALLBACKS
thomasgg 2:65e922b3569d 125 void motor0_callback(const std_msgs::Float32& msg){
thomasgg 2:65e922b3569d 126 motor0_sp = msg.data;
thomasgg 2:65e922b3569d 127 }
thomasgg 2:65e922b3569d 128
thomasgg 2:65e922b3569d 129 void motor1_callback(const std_msgs::Float32& msg){
thomasgg 2:65e922b3569d 130 motor1_sp = msg.data;
thomasgg 2:65e922b3569d 131 }
thomasgg 2:65e922b3569d 132
thomasgg 2:65e922b3569d 133 void motor2_callback(const std_msgs::Float32& msg){
thomasgg 2:65e922b3569d 134 motor2_sp = msg.data;
thomasgg 2:65e922b3569d 135 }
thomasgg 2:65e922b3569d 136
thomasgg 2:65e922b3569d 137 void motor3_callback(const std_msgs::Float32& msg){
thomasgg 2:65e922b3569d 138 motor3_sp = msg.data;
thomasgg 2:65e922b3569d 139 }
thomasgg 1:ac92cf476127 140
thomasgg 1:ac92cf476127 141
thomasgg 1:ac92cf476127 142
thomasgg 0:f0117a02cd54 143 // ===== LED
thomasgg 1:ac92cf476127 144 //DigitalOut myled(LED2);
thomasgg 1:ac92cf476127 145 //DigitalOut led = LED1;
thomasgg 0:f0117a02cd54 146
thomasgg 0:f0117a02cd54 147 // ===== ROSSERIAL SETUP
thomasgg 0:f0117a02cd54 148 ros::NodeHandle nh;
thomasgg 0:f0117a02cd54 149
thomasgg 1:ac92cf476127 150 // Encoder publisher
thomasgg 0:f0117a02cd54 151 std_msgs::Float32 enc_pos0;
thomasgg 1:ac92cf476127 152 ros::Publisher encoderP0("hardware/encoders/drive0", &enc_pos0);
thomasgg 0:f0117a02cd54 153
thomasgg 0:f0117a02cd54 154 std_msgs::Float32 enc_pos1;
thomasgg 1:ac92cf476127 155 ros::Publisher encoderP1("hardware/encoders/drive1", &enc_pos1);
thomasgg 0:f0117a02cd54 156
thomasgg 0:f0117a02cd54 157 std_msgs::Float32 enc_pos2;
thomasgg 1:ac92cf476127 158 ros::Publisher encoderP2("hardware/encoders/drive2", &enc_pos2);
thomasgg 0:f0117a02cd54 159
thomasgg 0:f0117a02cd54 160 std_msgs::Float32 enc_pos3;
thomasgg 1:ac92cf476127 161 ros::Publisher encoderP3("hardware/encoders/drive3", &enc_pos3);
thomasgg 1:ac92cf476127 162
thomasgg 1:ac92cf476127 163
thomasgg 1:ac92cf476127 164 // Drive subscribers
thomasgg 1:ac92cf476127 165 ros::Subscriber<std_msgs::Float32> motor0("hardware/motors/drive0", motor0_callback);
thomasgg 1:ac92cf476127 166 ros::Subscriber<std_msgs::Float32> motor1("hardware/motors/drive1", motor1_callback);
thomasgg 1:ac92cf476127 167 ros::Subscriber<std_msgs::Float32> motor2("hardware/motors/drive2", motor2_callback);
thomasgg 1:ac92cf476127 168 ros::Subscriber<std_msgs::Float32> motor3("hardware/motors/drive3", motor3_callback);
thomasgg 0:f0117a02cd54 169
thomasgg 0:f0117a02cd54 170 // ===== MAIN
thomasgg 0:f0117a02cd54 171 int main() {
thomasgg 1:ac92cf476127 172
thomasgg 0:f0117a02cd54 173 nh.initNode();
thomasgg 0:f0117a02cd54 174 nh.advertise(encoderP0);
thomasgg 0:f0117a02cd54 175 nh.advertise(encoderP1);
thomasgg 0:f0117a02cd54 176 nh.advertise(encoderP2);
thomasgg 0:f0117a02cd54 177 nh.advertise(encoderP3);
thomasgg 1:ac92cf476127 178
thomasgg 1:ac92cf476127 179 nh.subscribe(motor0);
thomasgg 1:ac92cf476127 180 nh.subscribe(motor1);
thomasgg 1:ac92cf476127 181 nh.subscribe(motor2);
thomasgg 1:ac92cf476127 182 nh.subscribe(motor3);
thomasgg 1:ac92cf476127 183
thomasgg 0:f0117a02cd54 184 while (1) {
thomasgg 0:f0117a02cd54 185
thomasgg 2:65e922b3569d 186 //dt = (millis() / 1000.0) - prev_t;
thomasgg 2:65e922b3569d 187 //prev_t = (millis() / 1000.0);
thomasgg 0:f0117a02cd54 188
thomasgg 2:65e922b3569d 189 if(count >= pub_skip){
thomasgg 2:65e922b3569d 190
thomasgg 2:65e922b3569d 191 // ENCODER P4
thomasgg 2:65e922b3569d 192 wheel0 = (float) encP0.getPulses() / pulses_per_wheel_rad;
thomasgg 2:65e922b3569d 193 setMotor0Power( -kP * ((wheel0 - wheel0_prev)/dt - motor0_sp) + kF * motor0_sp);
thomasgg 2:65e922b3569d 194 enc_pos0.data = wheel0;
thomasgg 2:65e922b3569d 195 encoderP0.publish( &enc_pos0 );
thomasgg 2:65e922b3569d 196 wheel0_prev = wheel0;
thomasgg 2:65e922b3569d 197
thomasgg 2:65e922b3569d 198 // ENCODER P3
thomasgg 2:65e922b3569d 199 wheel1 = (float) encP1.getPulses() / pulses_per_wheel_rad;
thomasgg 2:65e922b3569d 200 setMotor1Power( -kP * ((wheel1 - wheel1_prev)/dt - motor1_sp) + kF * motor1_sp);
thomasgg 2:65e922b3569d 201 enc_pos1.data = wheel1;
thomasgg 2:65e922b3569d 202 encoderP1.publish( &enc_pos1 );
thomasgg 2:65e922b3569d 203 wheel1_prev = wheel1;
thomasgg 2:65e922b3569d 204
thomasgg 2:65e922b3569d 205 // ENCODER P2
thomasgg 2:65e922b3569d 206 wheel2 = (float) encP2.getPulses() / pulses_per_wheel_rad;
thomasgg 2:65e922b3569d 207 setMotor2Power( -kP * ((wheel2 - wheel2_prev)/dt - motor2_sp) + kF * motor2_sp);
thomasgg 2:65e922b3569d 208 enc_pos2.data = wheel2;
thomasgg 2:65e922b3569d 209 encoderP2.publish( &enc_pos2 );
thomasgg 2:65e922b3569d 210 wheel2_prev = wheel2;
thomasgg 2:65e922b3569d 211
thomasgg 2:65e922b3569d 212 // ENCODER P3
thomasgg 2:65e922b3569d 213 wheel3 = -(float) encP3.getPulses() / pulses_per_wheel_rad;
thomasgg 2:65e922b3569d 214 setMotor3Power( -kP * ((wheel3 - wheel3_prev)/dt - motor3_sp) + kF * motor3_sp);
thomasgg 2:65e922b3569d 215 enc_pos3.data = wheel3;
thomasgg 2:65e922b3569d 216 encoderP3.publish( &enc_pos3 );
thomasgg 2:65e922b3569d 217 wheel3_prev = wheel3;
thomasgg 2:65e922b3569d 218
thomasgg 2:65e922b3569d 219 count = 0;
thomasgg 0:f0117a02cd54 220
thomasgg 2:65e922b3569d 221 }
thomasgg 0:f0117a02cd54 222
thomasgg 0:f0117a02cd54 223
thomasgg 0:f0117a02cd54 224 // Spin the rosserial node handler
thomasgg 0:f0117a02cd54 225 nh.spinOnce();
thomasgg 0:f0117a02cd54 226
thomasgg 2:65e922b3569d 227 wait(dt); // wait.....
thomasgg 2:65e922b3569d 228 count ++;
thomasgg 0:f0117a02cd54 229 }
thomasgg 0:f0117a02cd54 230 }