Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Abs enc #121

Open
wants to merge 2 commits into
base: master
Choose a base branch
from
Open

Abs enc #121

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
5a46d60
changing max to min
nathanestill Oct 19, 2019
abec2f6
adding absolute encoder software
nathanestill Nov 9, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion launch/hindbrain.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
<launch>
<node pkg="rosserial_python" type="serial_node.py" name="hindbrain" output="screen">
<param name="port" value="/dev/teensy"/>
<param name="port" value="/dev/ttyACM0"/>
<param name="baud" value="115200"/>
</node>
<node pkg="gravl" type="Watchdog" name="watchdog"/>
Expand Down
2 changes: 1 addition & 1 deletion package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,7 @@
<exec_depend>joy</exec_depend>
<exec_depend>phidgets_imu</exec_depend>
<exec_depend>nmea_navsat_driver</exec_depend>
<exec_depend>rosserial</exec_depend>
<!-- <exec_depend>rosserial</exec_depend> -->
<exec_depend>urg_node</exec_depend>
<exec_depend>robot_localization</exec_depend>

Expand Down
11 changes: 10 additions & 1 deletion src/hind_brain/hind_brain.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,11 +12,13 @@

// Libraries
#include "RoboClaw.h" // Used for motor controller interface
#include <Arduino.h> // Used for Arduino functions
#include <Arduino.h> // 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 <Encoder.h> // Used for hitch height feedback
Expand All @@ -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
Expand Down Expand Up @@ -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();
Expand Down
116 changes: 115 additions & 1 deletion src/hind_brain/hind_brain.ino
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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<ackermann_msgs::AckermannDrive> drive_sub("/cmd_vel", &ackermannCB);
ros::Subscriber<geometry_msgs::Pose> hitch_sub("/cmd_hitch", &hitchCB);
ros::Subscriber<std_msgs::Empty> ping("/safety_clock", &watchdogCB);
ros::Subscriber<std_msgs::Float64> wheelResetRight("/wheel_reset_right", &wheelRRCB);
ros::Subscriber<std_msgs::Float64> 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() {

Expand All @@ -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){
Expand All @@ -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();
Expand All @@ -106,6 +161,7 @@ void loop() {
// Update current published pose
updateCurrDrive();
updateCurrHitchPose();
updateWheelPose();

hitchMsg = computeHitchMsg();

Expand Down Expand Up @@ -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/
Expand Down Expand Up @@ -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;
Expand All @@ -226,7 +340,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
Expand Down