Spirit Dagozilla / Mbed OS BaseControlF7

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   dagozilla_utils_isaac cmps_dagoz motor_dagoz BaseControlF7 encoder_dagoz EncoderMotorInterrupt ros_lib_melodic_test

Dependents:   BaseControlF7

main.cpp@1:8b6487805a90, 2019-02-28 (annotated)

Committer:
calmantara186
Date:
Thu Feb 28 11:00:40 2019 +0000
Revision:
1:8b6487805a90
Parent:
0:1c22457d4aed
Child:
2:830d3c808679
Initial Commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
calmantara186 0:1c22457d4aed 1 #include "mbed.h"
calmantara186 1:8b6487805a90 2 #include "CMPS_DAGOZ.h"
calmantara186 1:8b6487805a90 3 #include "EncoderDAGOZ.h"
calmantara186 1:8b6487805a90 4 #include "MotorDAGOZ.h"
calmantara186 1:8b6487805a90 5 #include "EncoderMotor.h"
calmantara186 1:8b6487805a90 6 #include "PID.h"
calmantara186 1:8b6487805a90 7 #include "Constanta.h"
calmantara186 1:8b6487805a90 8 #include <ros.h>
calmantara186 1:8b6487805a90 9 #include <rosserial_mbed/HardwareCommandMsg.h>
calmantara186 1:8b6487805a90 10 #include <rosserial_mbed/HardwareStateMsg.h>
calmantara186 1:8b6487805a90 11
calmantara186 1:8b6487805a90 12 /*****************************
calmantara186 1:8b6487805a90 13 ROS node handle
calmantara186 1:8b6487805a90 14 *****************************/
calmantara186 1:8b6487805a90 15 ros::NodeHandle nh;
calmantara186 1:8b6487805a90 16
calmantara186 1:8b6487805a90 17 /*****************************
calmantara186 1:8b6487805a90 18 Pin Declaration
calmantara186 1:8b6487805a90 19 *****************************/
calmantara186 1:8b6487805a90 20 //Encoder pin
calmantara186 1:8b6487805a90 21 EncoderDAGOZ locomotionEncR(TIM1); //Locomotion Right Encoder
calmantara186 1:8b6487805a90 22 EncoderDAGOZ locomotionEncL(TIM3); //Locomotion Left Encoder
calmantara186 1:8b6487805a90 23 EncoderDAGOZ locomotionEncB(TIM2); //Locomotion Back Encoder
calmantara186 1:8b6487805a90 24 EncoderDAGOZ dribblerEncR(TIM4); //Dribbler Right Encoder
calmantara186 1:8b6487805a90 25 EncoderDAGOZ dribblerEncL(TIM8); //Dribbler Left Encoder
calmantara186 1:8b6487805a90 26
calmantara186 1:8b6487805a90 27 EncoderMotor intEncL(PE_0, PG_8, 537.6, EncoderMotor::X4_ENCODING);
calmantara186 1:8b6487805a90 28 EncoderMotor intEncR(PG_14, PD_10, 537.6, EncoderMotor::X4_ENCODING);
calmantara186 1:8b6487805a90 29 EncoderMotor intEncB(PF_1, PF_2, 537.6, EncoderMotor::X4_ENCODING);
calmantara186 1:8b6487805a90 30
calmantara186 1:8b6487805a90 31 //Motor pin buat Board Sistem next ver.
calmantara186 1:8b6487805a90 32 MotorDagoz locomotionMotorL(PF_12, PF_8); //Locomotion Right Motor
calmantara186 1:8b6487805a90 33 MotorDagoz locomotionMotorR(PG_1, PF_9); //Locomotion Left Motor
calmantara186 1:8b6487805a90 34 MotorDagoz locomotionMotorB(PF_11, PF_7); //Locomotion Back Motor
calmantara186 1:8b6487805a90 35
calmantara186 1:8b6487805a90 36 MotorDagoz dribblerMotorR(PF_15, PE_6); //Dribbler Right Motor
calmantara186 1:8b6487805a90 37 MotorDagoz dribblerMotorL(PE_15, PE_5); //Dribbler Left Motor
calmantara186 1:8b6487805a90 38
calmantara186 1:8b6487805a90 39
calmantara186 1:8b6487805a90 40 //Serial pin
calmantara186 1:8b6487805a90 41 Serial pc(USBTX, USBRX, 115200); //Serial debug
calmantara186 1:8b6487805a90 42 //Compass CMPS12 pin
calmantara186 1:8b6487805a90 43 CMPS_DAGOZ compass(PB_9, PB_8, 0xC0); //Compass I2C Communication
calmantara186 1:8b6487805a90 44 DigitalOut compassLed(PC_5); //CMPS error indicator
calmantara186 0:1c22457d4aed 45
calmantara186 1:8b6487805a90 46 //Potensio Pin
calmantara186 1:8b6487805a90 47 AnalogIn dribblerPotR(PF_6); //Potensio for Left Dribbler, di board saat ini masih PC_2
calmantara186 1:8b6487805a90 48 AnalogIn dribblerPotL(PC_3); //Potensio for Right Dribbler
calmantara186 1:8b6487805a90 49 AnalogIn kickerPot(PF_3); //Potensio for Kicker, di board saat ini masih PD_4
calmantara186 1:8b6487805a90 50
calmantara186 1:8b6487805a90 51 //LED Pin
calmantara186 1:8b6487805a90 52 DigitalOut ledRed(PG_4); //Red LED
calmantara186 1:8b6487805a90 53 DigitalOut ledGreen(PG_5); //Green LED
calmantara186 1:8b6487805a90 54 DigitalOut ledBlue(PG_6); //Blue LED
calmantara186 1:8b6487805a90 55 //Infrared Pin
calmantara186 1:8b6487805a90 56 AnalogIn infraRed(PF_5); //Ball distance IR, belum ada di board saat ini
calmantara186 1:8b6487805a90 57 //Kicker Pin
calmantara186 1:8b6487805a90 58 PwmOut kicker(PB_15); //Kicker pwm effort, di board saat ini masih PC_8
calmantara186 1:8b6487805a90 59 //Stepper Pin
calmantara186 1:8b6487805a90 60 DigitalOut stepperDir(PA_10); //Direction pin for stepper
calmantara186 1:8b6487805a90 61 DigitalOut stepperStep(PF_10); //Step pin for stepper, di board saat ini masih PC_4
calmantara186 1:8b6487805a90 62
calmantara186 1:8b6487805a90 63 /******************************
calmantara186 1:8b6487805a90 64 Publisher-Subscriber
calmantara186 1:8b6487805a90 65 ******************************/
calmantara186 1:8b6487805a90 66 /*dagozilla_msgs::HardwareStateMsg stateMsg;
calmantara186 1:8b6487805a90 67 ros::Publisher statePub("/hardware/state", &stateMsg);
calmantara186 0:1c22457d4aed 68
calmantara186 1:8b6487805a90 69 void commandCallback(const dagozilla_msgs::HardwareCommandMsg& commandMsg);
calmantara186 1:8b6487805a90 70 ros::Subscriber<dagozilla_msgs::HardwareCommandMsg> commandSub("/hardware/command", &commandCallback);*/
calmantara186 1:8b6487805a90 71
calmantara186 1:8b6487805a90 72 //PID global object
calmantara186 1:8b6487805a90 73 PID locomotionPidR(0.42210959642179, 0, 0, 100, 0.02); //Right locomotion PID
calmantara186 1:8b6487805a90 74 PID locomotionPidL(0.408433282254394, 0, 0, 100, 0.02); //Left locomotion PID
calmantara186 1:8b6487805a90 75 PID locomotionPidB(0.409120668853316, 0, 0, 100, 0.02); //Back locomotion PID
calmantara186 1:8b6487805a90 76 // PID dribblerPidL(0.279684770785072, 7.8707254128385, 0, 100, 0.02); //Right dribbler PID
calmantara186 1:8b6487805a90 77 // PID dribblerPidR(0.275600964975253, 7.51195777371376, 0, 100, 0.02); //Left dribbler PID
calmantara186 1:8b6487805a90 78
calmantara186 1:8b6487805a90 79 //thread for RTOS
calmantara186 1:8b6487805a90 80 Thread thread1;
calmantara186 1:8b6487805a90 81 Thread thread2;
calmantara186 1:8b6487805a90 82
calmantara186 1:8b6487805a90 83 //primitive function
calmantara186 1:8b6487805a90 84 void mainProcess();
calmantara186 1:8b6487805a90 85 void getCompass();
calmantara186 1:8b6487805a90 86 void moveLocomotion();
calmantara186 1:8b6487805a90 87 void moveDribbler();
calmantara186 1:8b6487805a90 88 void publishMessage();
calmantara186 1:8b6487805a90 89
calmantara186 1:8b6487805a90 90 /*****************************
calmantara186 1:8b6487805a90 91 Main Function
calmantara186 1:8b6487805a90 92 *****************************/
calmantara186 1:8b6487805a90 93
calmantara186 1:8b6487805a90 94 int main()
calmantara186 0:1c22457d4aed 95 {
calmantara186 1:8b6487805a90 96 //init ros advertise and subscribe
calmantara186 1:8b6487805a90 97 // nh.initNode();
calmantara186 1:8b6487805a90 98 // nh.advertise(statePub);
calmantara186 1:8b6487805a90 99 // nh.subscribe(commandSub);
calmantara186 0:1c22457d4aed 100
calmantara186 1:8b6487805a90 101 kicker.period(0.01f);
calmantara186 1:8b6487805a90 102 kicker = 1; //deactivate kicker, active LOW
calmantara186 0:1c22457d4aed 103
calmantara186 1:8b6487805a90 104 thread1.start(mainProcess);
calmantara186 1:8b6487805a90 105 // thread2.start(getCompass);
calmantara186 1:8b6487805a90 106
calmantara186 0:1c22457d4aed 107 while (true) {
calmantara186 1:8b6487805a90 108 //do nothing
calmantara186 0:1c22457d4aed 109 }
calmantara186 0:1c22457d4aed 110 }
calmantara186 0:1c22457d4aed 111
calmantara186 1:8b6487805a90 112 void mainProcess(){
calmantara186 1:8b6487805a90 113 float cur_pot_L0 = ((float)dribblerPotL.read()*100.0f);
calmantara186 1:8b6487805a90 114 float cur_pot_R0 = ((float)dribblerPotR.read()*100.0f);
calmantara186 1:8b6487805a90 115 Thread::wait(1000);
calmantara186 1:8b6487805a90 116
calmantara186 1:8b6487805a90 117 int rotR, rotL, rotB;
calmantara186 1:8b6487805a90 118 int rotInR, rotInL, rotInB;
calmantara186 1:8b6487805a90 119 int rotDbR, rotDbL;
calmantara186 1:8b6487805a90 120
calmantara186 1:8b6487805a90 121 while(1){
calmantara186 1:8b6487805a90 122 //nh.spinOnce();
calmantara186 1:8b6487805a90 123 cur_pot_L = ((float)dribblerPotL.read()*100.0f-cur_pot_L0);
calmantara186 1:8b6487805a90 124 cur_pot_R = ((float)dribblerPotR.read()*100.0f-cur_pot_R0);
calmantara186 1:8b6487805a90 125 //ball distance from IR
calmantara186 1:8b6487805a90 126 // distance = infraRed.read();
calmantara186 1:8b6487805a90 127 //read encoder
calmantara186 1:8b6487805a90 128 // cur_locomotion_R = -locomotionEncR.GetCounter(1)/4000.0;
calmantara186 1:8b6487805a90 129 // cur_locomotion_L = -locomotionEncL.GetCounter(1)/4000.0;
calmantara186 1:8b6487805a90 130 // cur_locomotion_B = -locomotionEncB.GetCounter(1)/4000.0;
calmantara186 1:8b6487805a90 131
calmantara186 1:8b6487805a90 132 rotR = locomotionEncR.GetCounter(0);
calmantara186 1:8b6487805a90 133 rotL = locomotionEncL.GetCounter(0);
calmantara186 1:8b6487805a90 134 rotB = locomotionEncB.GetCounter(0);
calmantara186 1:8b6487805a90 135 rotDbR = dribblerEncR.GetCounter(0);
calmantara186 1:8b6487805a90 136 rotDbL = dribblerEncL.GetCounter(0);
calmantara186 1:8b6487805a90 137
calmantara186 1:8b6487805a90 138 rotInR += intEncR.getPulses();
calmantara186 1:8b6487805a90 139 rotInL += intEncL.getPulses();
calmantara186 1:8b6487805a90 140 rotInB += intEncB.getPulses();
calmantara186 1:8b6487805a90 141
calmantara186 1:8b6487805a90 142 // cur_dribbler_R = dribblerEncR.GetCounter(1)/537.6;
calmantara186 1:8b6487805a90 143 // cur_dribbler_L = dribblerEncL.GetCounter(1)/537.6;
calmantara186 1:8b6487805a90 144 // Correct encoder unit
calmantara186 1:8b6487805a90 145 locomotion_R_rot = cur_locomotion_R * LOCOMOTIONWHEEL * 2 * PI; // in m
calmantara186 1:8b6487805a90 146 locomotion_L_rot = cur_locomotion_L * LOCOMOTIONWHEEL * 2 * PI; // in m
calmantara186 1:8b6487805a90 147 locomotion_B_rot = cur_locomotion_B * LOCOMOTIONWHEEL * 2 * PI; // in m
calmantara186 1:8b6487805a90 148 dribbler_R_rot = cur_dribbler_L * DRIBBLERWHEEL * 2 * PI; // in m
calmantara186 1:8b6487805a90 149 dribbler_L_rot = cur_dribbler_R * DRIBBLERWHEEL * 2 * PI; // in m
calmantara186 1:8b6487805a90 150 // Calculate acutal velocity
calmantara186 1:8b6487805a90 151 locomotion_R_vel = locomotion_R_rot / 0.002;
calmantara186 1:8b6487805a90 152 locomotion_L_vel = locomotion_L_rot / 0.002;
calmantara186 1:8b6487805a90 153 locomotion_B_vel = locomotion_B_rot / 0.002;
calmantara186 1:8b6487805a90 154 dribbler_R_vel = dribbler_R_rot / 0.002;
calmantara186 1:8b6487805a90 155 dribbler_L_vel = dribbler_L_rot / 0.002;
calmantara186 0:1c22457d4aed 156
calmantara186 1:8b6487805a90 157 //movement
calmantara186 1:8b6487805a90 158 //moveLocomotion();
calmantara186 1:8b6487805a90 159 //moveDribbler();
calmantara186 1:8b6487805a90 160
calmantara186 1:8b6487805a90 161 //kicker conditional
calmantara186 1:8b6487805a90 162 // if(kick_power_target > 0) {
calmantara186 1:8b6487805a90 163 // kicker.write(1-kick_power_target); //Active LOW
calmantara186 1:8b6487805a90 164 // Thread::wait(10);
calmantara186 1:8b6487805a90 165 // kicker = 1;
calmantara186 1:8b6487805a90 166 // kick_power_target = 0;
calmantara186 1:8b6487805a90 167 // }
calmantara186 0:1c22457d4aed 168
calmantara186 1:8b6487805a90 169 // publishMessage();
calmantara186 1:8b6487805a90 170 // nh.spinOnce();
calmantara186 1:8b6487805a90 171 pc.printf("%d, %d\n", rotDbR, rotDbL);
calmantara186 1:8b6487805a90 172 // pc.printf("vx:%.2f vy:%.2f w:%.2f dR:%.2f dL:%.2f pR:%.2f pL:%.2f\n", vx, vy, w, dribbler_R_target_rate, dribbler_L_target_rate, cur_pot_R, cur_pot_L);
calmantara186 1:8b6487805a90 173
calmantara186 1:8b6487805a90 174 // locomotionMotorR.setpwm(0.3);
calmantara186 1:8b6487805a90 175 // locomotionMotorB.setpwm(0.3);
calmantara186 1:8b6487805a90 176 // locomotionMotorL.setpwm(0.3);
calmantara186 1:8b6487805a90 177
calmantara186 1:8b6487805a90 178 // dribblerMotorR.setpwm(0.3);
calmantara186 1:8b6487805a90 179 // dribblerMotorL.setpwm(0.3);
calmantara186 1:8b6487805a90 180 Thread::wait(10);
calmantara186 0:1c22457d4aed 181 }
calmantara186 0:1c22457d4aed 182 }
calmantara186 1:8b6487805a90 183
calmantara186 1:8b6487805a90 184 void getCompass(){
calmantara186 1:8b6487805a90 185 float theta0 = compass.readBearing()/10.0;
calmantara186 1:8b6487805a90 186 Thread::wait(1000);
calmantara186 1:8b6487805a90 187 // theta0 = compass.readBearing()/10.0;
calmantara186 1:8b6487805a90 188 compassLed = 0;
calmantara186 1:8b6487805a90 189 while(1){
calmantara186 1:8b6487805a90 190 float theta_temp = (compass.readBearing()/10.0) - theta0;
calmantara186 1:8b6487805a90 191 if(theta_temp > 180.0 && theta_temp <= 360.0)
calmantara186 1:8b6487805a90 192 theta_com = ((compass.readBearing()/10.0) - 360.0 - theta0)/-RADTODEG;
calmantara186 1:8b6487805a90 193 else if(theta_temp < -180.0 && theta_temp >= -360.0)
calmantara186 1:8b6487805a90 194 theta_com = ((compass.readBearing()/10.0) + 360.0 - theta0)/-RADTODEG;
calmantara186 1:8b6487805a90 195 else
calmantara186 1:8b6487805a90 196 theta_com = ((compass.readBearing()/10.0) - theta0)/-RADTODEG;
calmantara186 1:8b6487805a90 197
calmantara186 1:8b6487805a90 198 if(theta != 0) compassLed = 1;
calmantara186 1:8b6487805a90 199 else compassLed = 0;
calmantara186 1:8b6487805a90 200
calmantara186 1:8b6487805a90 201 theta = theta_com;
calmantara186 1:8b6487805a90 202 theta_prev = theta_com;
calmantara186 1:8b6487805a90 203
calmantara186 1:8b6487805a90 204 Thread::wait(50);
calmantara186 1:8b6487805a90 205 }
calmantara186 1:8b6487805a90 206 }
calmantara186 1:8b6487805a90 207
calmantara186 1:8b6487805a90 208 void moveLocomotion(){
calmantara186 1:8b6487805a90 209 // Calculate motor pwm
calmantara186 1:8b6487805a90 210 locomotion_R_target_rate = locomotionPidR.createpwm(locomotion_R_vtarget, locomotion_R_vel);
calmantara186 1:8b6487805a90 211 locomotion_L_target_rate = locomotionPidL.createpwm(locomotion_L_vtarget, locomotion_L_vel);
calmantara186 1:8b6487805a90 212 locomotion_B_target_rate = locomotionPidB.createpwm(locomotion_B_vtarget, locomotion_B_vel);
calmantara186 1:8b6487805a90 213 //Compute value
calmantara186 1:8b6487805a90 214 x = x_prev + (2*locomotion_B_rot - locomotion_L_rot - locomotion_R_rot)/3*cos(theta_prev) - (-locomotion_L_rot+locomotion_R_rot)*0.5773*sin(theta_prev);
calmantara186 1:8b6487805a90 215 y = y_prev + (2*locomotion_B_rot - locomotion_L_rot - locomotion_R_rot)/3*sin(theta_prev) + (-locomotion_L_rot+locomotion_R_rot)*0.5773*cos(theta_prev);
calmantara186 1:8b6487805a90 216 theta = theta_prev + (locomotion_R_rot + locomotion_B_rot + locomotion_L_rot)/(3*0.116);
calmantara186 1:8b6487805a90 217 //update value
calmantara186 1:8b6487805a90 218 x_prev = x; y_prev = y; theta_prev = theta;
calmantara186 1:8b6487805a90 219 //Robot Velocity
calmantara186 1:8b6487805a90 220 vx = (x - x_prev)/0.01; vy = (y - y_prev)/0.01; w = (theta - theta_prev)/0.01;
calmantara186 1:8b6487805a90 221 //Motor pwm
calmantara186 1:8b6487805a90 222 locomotionMotorR.setpwm(locomotion_R_target_rate);
calmantara186 1:8b6487805a90 223 locomotionMotorL.setpwm(locomotion_L_target_rate);
calmantara186 1:8b6487805a90 224 locomotionMotorB.setpwm(locomotion_B_target_rate);
calmantara186 1:8b6487805a90 225 }
calmantara186 1:8b6487805a90 226
calmantara186 1:8b6487805a90 227 void moveDribbler(){
calmantara186 1:8b6487805a90 228 float RKXFF, LKXFF;
calmantara186 1:8b6487805a90 229 if (vx <= 0){
calmantara186 1:8b6487805a90 230 RKXFF = 1;
calmantara186 1:8b6487805a90 231 LKXFF = 0.5;
calmantara186 1:8b6487805a90 232 } else{
calmantara186 1:8b6487805a90 233 RKXFF = 0.5;
calmantara186 1:8b6487805a90 234 LKXFF = 1;
calmantara186 1:8b6487805a90 235 }
calmantara186 1:8b6487805a90 236 //Calculate Feedback and FeedForward
calmantara186 1:8b6487805a90 237 //need correction for +- in each robot velocity
calmantara186 1:8b6487805a90 238 dribbler_R_target_rate = (abs(LTARGET - cur_pot_L)*LK1FB)+(abs(vx*LKXFF) - (vy*LKYFF) + abs(w*LKTFF));
calmantara186 1:8b6487805a90 239 dribbler_L_target_rate = (abs(RTARGET - cur_pot_R)*RK1FB)+(abs(vx*RKXFF) - (vy*RKYFF) + abs(w*RKTFF));
calmantara186 1:8b6487805a90 240 //dribbler_L_target_rate = ((targetVelL - dribblerLVel)*LK2FB)+(LKVFF*(targetVelL-prevTargetVelL));
calmantara186 1:8b6487805a90 241 //dribbler_R_target_rate = ((targetVelR - dribblerRVel)*RK2FB)+(RKVFF*(targetVelR-prevTargetVelR));
calmantara186 1:8b6487805a90 242 // prevTargetVelL = targetVelL;
calmantara186 1:8b6487805a90 243 // prevTargetVelR = targetVelR;
calmantara186 1:8b6487805a90 244 if(dribbler_R_target_rate>1.0) dribbler_R_target_rate = 1.0;
calmantara186 1:8b6487805a90 245 if(dribbler_L_target_rate>1.0) dribbler_L_target_rate = 1.0;
calmantara186 1:8b6487805a90 246
calmantara186 1:8b6487805a90 247 if(abs(LTARGET - cur_pot_L)<=2.0) dribbler_R_target_rate = 0.0;
calmantara186 1:8b6487805a90 248 if(abs(RTARGET - cur_pot_R)<=2.0) dribbler_L_target_rate = 0.0;
calmantara186 1:8b6487805a90 249
calmantara186 1:8b6487805a90 250 dribblerMotorL.setpwm(dribbler_L_target_rate);
calmantara186 1:8b6487805a90 251 dribblerMotorR.setpwm(dribbler_R_target_rate);
calmantara186 1:8b6487805a90 252 }
calmantara186 1:8b6487805a90 253
calmantara186 1:8b6487805a90 254 void publishMessage(){
calmantara186 1:8b6487805a90 255 // Publish position data
calmantara186 1:8b6487805a90 256 //stateMsg.base_wheel_right_position = locomotion_R_rot; // in m
calmantara186 1:8b6487805a90 257 // stateMsg.base_wheel_back_position = locomotion_B_rot; // in m
calmantara186 1:8b6487805a90 258 // stateMsg.base_wheel_left_position = locomotion_L_rot; // in m
calmantara186 1:8b6487805a90 259 // stateMsg.dribbler_wheel_left_position = dribbler_L_rot; // in m
calmantara186 1:8b6487805a90 260 // stateMsg.dribbler_wheel_right_position = dribbler_R_rot; // in m
calmantara186 1:8b6487805a90 261 // // Publish velocity data
calmantara186 1:8b6487805a90 262 // stateMsg.base_wheel_right_velocity = locomotion_R_vel; // in m/s
calmantara186 1:8b6487805a90 263 // stateMsg.base_wheel_back_velocity = locomotion_B_vel; // in m/s
calmantara186 1:8b6487805a90 264 // stateMsg.base_wheel_left_velocity = locomotion_L_vel; // in m/s
calmantara186 1:8b6487805a90 265 // stateMsg.dribbler_wheel_left_velocity = dribbler_L_vel; // in m/s
calmantara186 1:8b6487805a90 266 // stateMsg.dribbler_wheel_right_velocity = dribbler_R_vel; // in m/s
calmantara186 1:8b6487805a90 267 // // Publish distance data
calmantara186 1:8b6487805a90 268 // stateMsg.dribbler_ir_distance = distance; // in analog value
calmantara186 1:8b6487805a90 269 // statePub.publish(&stateMsg);
calmantara186 1:8b6487805a90 270 }
calmantara186 1:8b6487805a90 271
calmantara186 1:8b6487805a90 272 void commandCallback(const dagozilla_msgs::HardwareCommandMsg& commandMsg) {
calmantara186 1:8b6487805a90 273 locomotion_R_vtarget = commandMsg.base_wheel_right_velocity;
calmantara186 1:8b6487805a90 274 locomotion_B_vtarget = commandMsg.base_wheel_back_velocity;
calmantara186 1:8b6487805a90 275 locomotion_L_vtarget = commandMsg.base_wheel_left_velocity;
calmantara186 1:8b6487805a90 276 dribbler_L_vtarget = commandMsg.dribbler_wheel_left_velocity;
calmantara186 1:8b6487805a90 277 dribbler_R_vtarget = commandMsg.dribbler_wheel_left_velocity;
calmantara186 1:8b6487805a90 278 kick_power_target = commandMsg.kicker_effort;
calmantara186 1:8b6487805a90 279 }