From 5a46d60c9ce1944a9135927387686f01c6ce3f22 Mon Sep 17 00:00:00 2001 From: nathanestill Date: Sat, 19 Oct 2019 16:58:55 -0400 Subject: [PATCH 1/2] changing max to min --- src/hind_brain/hind_brain.ino | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/hind_brain/hind_brain.ino b/src/hind_brain/hind_brain.ino index 83fc276..cd5d34c 100644 --- a/src/hind_brain/hind_brain.ino +++ b/src/hind_brain/hind_brain.ino @@ -226,7 +226,7 @@ int computeHitchMsg(){ } else{ if (error > 0){ // Hitch is too high - hitch_msg = H_ACTUATOR_MAX; // Move lever forwards + hitch down + hitch_msg = H_ACTUATOR_MIN; // Move lever forwards + hitch down } else { // Hitch is too low hitch_msg = H_ACTUATOR_MAX; // Move lever backwards + hitch up From abec2f682425ab98c351fbc01d4a64023d1b4bf7 Mon Sep 17 00:00:00 2001 From: nathanestill Date: Sat, 9 Nov 2019 18:00:34 -0500 Subject: [PATCH 2/2] adding absolute encoder software --- launch/hindbrain.launch | 2 +- package.xml | 2 +- src/hind_brain/hind_brain.h | 11 +++- src/hind_brain/hind_brain.ino | 114 ++++++++++++++++++++++++++++++++++ 4 files changed, 126 insertions(+), 3 deletions(-) diff --git a/launch/hindbrain.launch b/launch/hindbrain.launch index efc46b7..c1a5a1d 100644 --- a/launch/hindbrain.launch +++ b/launch/hindbrain.launch @@ -1,6 +1,6 @@ - + diff --git a/package.xml b/package.xml index 912400b..02c04cd 100644 --- a/package.xml +++ b/package.xml @@ -32,7 +32,7 @@ joy phidgets_imu nmea_navsat_driver - rosserial + urg_node robot_localization diff --git a/src/hind_brain/hind_brain.h b/src/hind_brain/hind_brain.h index 0e5b9a7..331ed11 100644 --- a/src/hind_brain/hind_brain.h +++ b/src/hind_brain/hind_brain.h @@ -12,11 +12,13 @@ // Libraries #include "RoboClaw.h" // Used for motor controller interface -#include // Used for Arduino functions +#include // Used for Arduino functions #include "ros.h" // Used for rosserial communication #include "ackermann_msgs/AckermannDrive.h" // Used for rosserial steering message #include "geometry_msgs/Pose.h" // Used for rosserial hitch message #include "std_msgs/Empty.h" // Used for watchdog hf connection monitor +#include "std_msgs/Bool.h" // Used for absolute encoder +#include "std_msgs/Float64.h" // Used for absolute encoder #include "estop.h" // Used to implement estop class #include "soft_switch.h" // Used to implement auto switch #include // Used for hitch height feedback @@ -27,6 +29,9 @@ const byte AUTO_LED_PIN = 3; const byte ESTOP_PIN = 2; const byte HITCH_ENC_A_PIN = 18; const byte HITCH_ENC_B_PIN = 19; +const byte WHEEL_ENC_R_PINS[6] = {26,27,28,29,30,31}; +const byte WHEEL_ENC_L_PINS[6] = {34,35,36,37,38,39}; + // Roboclaw Constants @@ -65,6 +70,10 @@ const float ENC_STOP_THRESHOLD = 0.0381; // Threshold of blade accuracy to stop void ackermannCB(const ackermann_msgs::AckermannDrive&); void hitchCB(const geometry_msgs::Pose&); void watchdogCB(const std_msgs::Empty&); +void wheelRRCB(const std_msgs::Float64&); +void wheelRLCB(const std_msgs::Float64&); + + void stopEngine(); void eStop(); void eStart(); diff --git a/src/hind_brain/hind_brain.ino b/src/hind_brain/hind_brain.ino index cd5d34c..b06e64e 100644 --- a/src/hind_brain/hind_brain.ino +++ b/src/hind_brain/hind_brain.ino @@ -24,6 +24,15 @@ RoboClaw rc2(rc2_serial, RC_TIMEOUT); // Encoders Encoder hitchEncoder(HITCH_ENC_A_PIN, HITCH_ENC_B_PIN); +//Wheel Lengths +float distanceR = 0.0; +float distanceL = 0.0; +int prevR = 0; +int prevL = 0; +int revsR = 0; +int revsL = 0; +int offsetR = 0; +int offsetL = 0; // States boolean isEStopped = false; @@ -43,11 +52,20 @@ ros::NodeHandle nh; ackermann_msgs::AckermannDrive curr_drive_pose; geometry_msgs::Pose curr_hitch_pose; geometry_msgs::Pose desired_hitch_pose; +std_msgs::Float64 curr_wheel_enc_right; +std_msgs::Float64 curr_wheel_enc_left; + ros::Subscriber drive_sub("/cmd_vel", &ackermannCB); ros::Subscriber hitch_sub("/cmd_hitch", &hitchCB); ros::Subscriber ping("/safety_clock", &watchdogCB); +ros::Subscriber wheelResetRight("/wheel_reset_right", &wheelRRCB); +ros::Subscriber wheelResetLeft("/wheel_reset_left", &wheelRLCB); ros::Publisher pub_drive("/curr_drive", &curr_drive_pose); ros::Publisher hitch_pose("/hitch_pose", &curr_hitch_pose); +ros::Publisher encoderRight("/wheel_enc_right", &curr_wheel_enc_right); +ros::Publisher encoderLeft("/wheel_enc_left", &curr_wheel_enc_left); + + void setup() { @@ -71,8 +89,13 @@ void setup() { nh.subscribe(drive_sub); nh.subscribe(hitch_sub); nh.subscribe(ping); + nh.subscribe(wheelResetRight); + nh.subscribe(wheelResetLeft); nh.advertise(pub_drive); nh.advertise(hitch_pose); + nh.advertise(encoderRight); + nh.advertise(encoderLeft); + // Wait for connection while(true){ @@ -93,6 +116,38 @@ void setup() { rc1.SpeedAccelDeccelPositionM2(RC1_ADDRESS, 0, 500, 0, steerMsg, 1); rc2.SpeedAccelDeccelPositionM2(RC2_ADDRESS, 0, 300, 0, hitchMsg, 1); + // Initialize Wheel Encoder Pins + for(int i = 0; i <=5; i++){ + pinMode(WHEEL_ENC_R_PINS[i],INPUT); + pinMode(WHEEL_ENC_L_PINS[i],INPUT); + } + + //Initialize the Wheel Encoders to 0 + bool readingsR[6]; + bool readingsL[6]; + readingsR[0] = !digitalRead(WHEEL_ENC_R_PINS[0]); + readingsR[1] = !digitalRead(WHEEL_ENC_R_PINS[1]); + readingsR[2] = !digitalRead(WHEEL_ENC_R_PINS[2]); + readingsR[3] = !digitalRead(WHEEL_ENC_R_PINS[3]); + readingsR[4] = !digitalRead(WHEEL_ENC_R_PINS[4]); + readingsR[5] = !digitalRead(WHEEL_ENC_R_PINS[5]); + offsetR = readingsR[5] * pow(2,5); + for(int i = 4; i >= 0; i--){ + readingsR[i] = readingsR[i+1] ^ readingsR[i]; + offsetR += readingsR[i] * pow(2,i); + } + readingsL[0] = !digitalRead(WHEEL_ENC_L_PINS[0]); + readingsL[1] = !digitalRead(WHEEL_ENC_L_PINS[1]); + readingsL[2] = !digitalRead(WHEEL_ENC_L_PINS[2]); + readingsL[3] = !digitalRead(WHEEL_ENC_L_PINS[3]); + readingsL[4] = !digitalRead(WHEEL_ENC_L_PINS[4]); + readingsL[5] = !digitalRead(WHEEL_ENC_L_PINS[5]); + offsetL = readingsL[5] * pow(2,5); + for(int i = 4; i >= 0; i--){ + readingsL[i] = readingsL[i+1] ^ readingsL[i]; + offsetL += readingsL[i] * pow(2,i); + } + // TODO: make wait until motors reach default positions watchdog_timer = millis(); @@ -106,6 +161,7 @@ void loop() { // Update current published pose updateCurrDrive(); updateCurrHitchPose(); + updateWheelPose(); hitchMsg = computeHitchMsg(); @@ -138,6 +194,16 @@ void watchdogCB(const std_msgs::Empty &msg) { watchdog_timer = millis(); } // watchdogCB() +void wheelRRCB(const std_msgs::Float64 &msg) { + revsR = int(msg.data / (0.8182 * 3.14)); + offsetR = prevR - (int(64.0 * msg.data/ (0.8182 * 3.14)) % 64); +} + +void wheelRLCB(const std_msgs::Float64 &msg) { + revsL = int(msg.data / (0.8182 * 3.14)); + offsetL = prevL - (int(64.0 * msg.data/ (0.8182 * 3.14)) % 64); +} + void checkSerial(ros::NodeHandle *nh) { // Given node, estops if watchdog has timed out // https://answers.ros.org/question/124481/rosserial-arduino-how-to-check-on-device-if-in-sync-with-host/ @@ -211,6 +277,54 @@ void updateCurrHitchPose(){ hitch_pose.publish(&curr_hitch_pose); } // updateCurrHitchPose() +void updateWheelPose(){ + bool readingsR[6]; + bool readingsL[6]; + int totalR; + int totalL; + readingsR[0] = !digitalRead(WHEEL_ENC_R_PINS[0]); + readingsR[1] = !digitalRead(WHEEL_ENC_R_PINS[1]); + readingsR[2] = !digitalRead(WHEEL_ENC_R_PINS[2]); + readingsR[3] = !digitalRead(WHEEL_ENC_R_PINS[3]); + readingsR[4] = !digitalRead(WHEEL_ENC_R_PINS[4]); + readingsR[5] = !digitalRead(WHEEL_ENC_R_PINS[5]); + totalR = readingsR[5] * pow(2,5); + for(int i = 4; i >= 0; i--){ + readingsR[i] = readingsR[i+1] ^ readingsR[i]; + totalR += readingsR[i] * pow(2,i); + } + readingsL[0] = !digitalRead(WHEEL_ENC_L_PINS[0]); + readingsL[1] = !digitalRead(WHEEL_ENC_L_PINS[1]); + readingsL[2] = !digitalRead(WHEEL_ENC_L_PINS[2]); + readingsL[3] = !digitalRead(WHEEL_ENC_L_PINS[3]); + readingsL[4] = !digitalRead(WHEEL_ENC_L_PINS[4]); + readingsL[5] = !digitalRead(WHEEL_ENC_L_PINS[5]); + totalL = readingsL[5] * pow(2,5); + for(int i = 4; i >= 0; i--){ + readingsL[i] = readingsL[i+1] ^ readingsL[i]; + totalL += readingsL[i] * pow(2,i); + } + if(totalR > 48 && prevR < 16){ + revsR--; + } + if(totalR < 16 && prevR > 48){ + revsR++; + } + if(totalL > 48 && prevL < 16){ + revsL--; + } + if(totalL < 16 && prevL > 48){ + revsL++; + } + prevL = totalL; + prevR = totalR; + curr_wheel_enc_right.data = (revsR * 64 + totalR - offsetR) * 0.8182 * 3.14 / 64.0; // diameter * pi / hits per rev + curr_wheel_enc_left.data = (revsL * 64 + totalL - offsetL) * 0.8182 * 3.14 / 64.0 ; + encoderRight.publish(&curr_wheel_enc_right); + encoderLeft.publish(&curr_wheel_enc_left); + +} + int computeHitchMsg(){ // Take current and desired hitch position to compute actuator position float desired = desired_hitch_pose.position.z;