Skip to content

Commit

Permalink
some minor changes for final testing
Browse files Browse the repository at this point in the history
  • Loading branch information
@NeilNie
NeilNie committed Sep 29, 2019
1 parent eb4cda6 commit 3eeaa62
Show file tree
Hide file tree
Showing 37 changed files with 302 additions and 147 deletions.
65 changes: 44 additions & 21 deletions .idea/workspace.xml
View file

Some generated files are not rendered by default. Learn more about how customized files appear on GitHub.

91 changes: 56 additions & 35 deletions arduino/brake_accelerator_controller/brake_accelerator_controller.ino
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,20 +43,22 @@ boolean killed = false;
boolean velocity_callback = false;
boolean joystick_enabled = false;
boolean go = false;
boolean rc_control = false;

int ch1; // Here's where we'll keep our channel values
int ch2;
int ch3;
int ch4;
int ch5;
int log_val;

void joystick_enabled_callback( const std_msgs::Bool& cmd_msg) {
joystick_enabled = cmd_msg.data;
}

void time_sync_callback( const std_msgs::Header& header_msg) {
current_vel_time = header_msg.stamp.nsec;
}

void killswitch_callback( const std_msgs::Bool& cmd_msg) {
killed = cmd_msg.data;
}
//void time_sync_callback( const std_msgs::Header& header_msg) {
// current_vel_time = header_msg.stamp.nsec;
//}

void set_go_status(const std_msgs::Bool &cmd_msg) {

Expand Down Expand Up @@ -90,18 +92,18 @@ void joystick_callback( const std_msgs::Float32& cmd_msg) {
// ========================================================
// ===================== R C Control ======================

void dir_accel_callback(const std_msgs::Float32& cmd_msg) {

// make sure the brake is released
if (!killed) {
if (cmd_msg.data < 0) {
remote_accel_val = 0;
float inverted_input = 1.0 + cmd_msg.data;
actuator_target_pos = mapf(inverted_input, 0, 1, LA_MIN, LA_MAX);
} else
remote_accel_val = mapf(cmd_msg.data, 0, 1, POT_MIN, POT_MID);
}
}
//void dir_accel_callback(const std_msgs::Float32& cmd_msg) {
//
// // make sure the brake is released
// if (!killed) {
// if (cmd_msg.data < 0) {
// remote_accel_val = 0;
// float inverted_input = 1.0 + cmd_msg.data;
// actuator_target_pos = mapf(inverted_input, 0, 1, LA_MIN, LA_MAX);
// } else
// remote_accel_val = mapf(cmd_msg.data, 0, 1, POT_MIN, POT_MID);
// }
//}

// =========================================================
// =========================================================
Expand Down Expand Up @@ -130,7 +132,7 @@ void desired_vel_callback(const std_msgs::Float32& cmd_msg) {
actuator_target_pos = LA_MIN + (LA_MAX - LA_MIN) * 0.45;
} else if (delta_vel <= -0.12 && delta_vel >= -0.25) {
log_val = 2;
actuator_target_pos = LA_MIN + (LA_MAX - LA_MIN) * 0.30;
actuator_target_pos = LA_MIN + (LA_MAX - LA_MIN) * 0.33;
// if dramatic deceleration or low speed
} else {
log_val = 3;
Expand All @@ -154,31 +156,40 @@ void desired_vel_callback(const std_msgs::Float32& cmd_msg) {
std_msgs::Int16 pos_msg;
ros::Publisher pos_pub("/sensor/vehicle/brake/actuator_position", &pos_msg);

std_msgs::Bool kill_msg;
ros::Publisher kill_pub("/vehicle/safety/killed", &kill_msg);

void setup() {

// create all subscribers
ros::Subscriber<std_msgs::Float32> time_sync("/vehicle/dbw/time_sync", time_sync_callback);
// ros::Subscriber<std_msgs::Float32> time_sync("/vehicle/dbw/time_sync", time_sync_callback);
ros::Subscriber<std_msgs::Float32> dvel_sub("/vehicle/dbw/desired_velocity", desired_vel_callback);
ros::Subscriber<std_msgs::Float32> cvel_sub("/vehicle/dbw/current_velocity", current_vel_callback);

ros::Subscriber<std_msgs::Bool> go_sub("/vehicle/dbw/go", set_go_status);
ros::Subscriber<std_msgs::Float32> sub2("/sensor/joystick/right_stick_y", joystick_callback);
ros::Subscriber<std_msgs::Bool> sub3("/sensor/joystick/enabled", joystick_enabled_callback);
ros::Subscriber<std_msgs::Bool> ks_sub("/vehicle/safety/killed", killswitch_callback);
ros::Subscriber<std_msgs::Float32> dir_accel_sub("/sensor/dbw/remote_velocity", dir_accel_callback);
// ros::Subscriber<std_msgs::Float32> dir_accel_sub("/sensor/dbw/remote_velocity", dir_accel_callback);

nh.initNode();

// subscribe to all topics
nh.subscribe(sub2);
nh.subscribe(sub3);
nh.subscribe(time_sync);
nh.subscribe(cvel_sub);
nh.subscribe(dvel_sub);
nh.subscribe(ks_sub);
nh.subscribe(dir_accel_sub);
nh.advertise(kill_pub);
// nh.subscribe(dir_accel_sub);

nh.advertise(pos_pub);

// RC receiver code
pinMode(2, INPUT);
pinMode(3, INPUT);
pinMode(4, INPUT);
pinMode(5, INPUT);
pinMode(8, INPUT);

pinMode(RPWM, OUTPUT);
pinMode(LPWM, OUTPUT);

Expand All @@ -195,16 +206,27 @@ void setup() {

void loop() {

if (killed)
actuator_target_pos = LA_MIN;
// Read the pulse width of
// ch1 = pulseIn(2, HIGH, 25000);
// ch2 = pulseIn(3, HIGH, 25000);
// ch3 = pulseIn(4, HIGH, 25000);
// ch4 = pulseIn(5, HIGH, 25000);
// ch5 = pulseIn(8, HIGH, 25000);

// if (ch5 >= 1600) {
// kill_msg.data = true;
// killed = true;
// } else {
// killed = false;
// kill_msg.data = false;
// }

// if (!killed && ((float(abs(current_vel_time - desired_vel_time)) / float(pow(10, 9))) >= 0.35)) {
// remote_accel_val = POT_MIN;
// actuator_target_pos = LA_MIN + ((LA_MAX - LA_MIN) * 0.90);
//
// log_string = -1;
// }
kill_pub.publish(&kill_msg);

// ==========================
if (killed)
actuator_target_pos = LA_MIN;

// accelerator control
if (joystick_enabled && !killed) {
potWrite(slave_Select_Pin, B00010001, cmd_val);
Expand Down Expand Up @@ -263,7 +285,6 @@ void move_actuator(int spd, boolean dir) {
void stop_actuator() {
analogWrite(LPWM, 0);
analogWrite(RPWM, 0);
delay(5);
}

double mapf(double x, double in_min, double in_max, double out_min, double out_max) {
Expand Down
12 changes: 6 additions & 6 deletions arduino/linear_actuator_controller/linear_actuator_controller.ino
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,7 @@ double pos = 0.0; // steering position
boolean joystick_enabled = false;
float cmd_val = 0.0;
float target_pos = 0.0;
bool killed;
bool killed = false;

ros::NodeHandle nh;

Expand Down Expand Up @@ -80,10 +80,6 @@ void killswitch_callback( const std_msgs::Bool& cmd_msg) {

// ----------------------------------------------------------------------------------------
// declare all subscribers
ros::Subscriber<std_msgs::Float32> sub1("/vehicle/dbw/steering_cmds/", steering_callback);
ros::Subscriber<std_msgs::Float32> sub2("/sensor/joystick/left_stick_x", joystick_callback);
ros::Subscriber<std_msgs::Bool> sub3("/sensor/joystick/enabled", joystick_enabled_callback);
ros::Subscriber<std_msgs::Bool> ks_sub("/vehicle/safety/killed", killswitch_callback);

// declare the publisher
std_msgs::Float32 pos_msg;
Expand All @@ -92,6 +88,11 @@ ros::Publisher pos_pub("/sensor/vehicle/steering/actuator_position", &pos_msg);

void setup() {

ros::Subscriber<std_msgs::Float32> sub1("/vehicle/dbw/steering_cmds/", steering_callback);
ros::Subscriber<std_msgs::Float32> sub2("/sensor/joystick/left_stick_x", joystick_callback);
ros::Subscriber<std_msgs::Bool> sub3("/sensor/joystick/enabled", joystick_enabled_callback);
ros::Subscriber<std_msgs::Bool> ks_sub("/vehicle/safety/killed", killswitch_callback);

nh.initNode();
nh.subscribe(sub1);
nh.subscribe(sub2);
Expand Down Expand Up @@ -154,7 +155,6 @@ void move_actuator(int spd, boolean dir) {
void stop_actuator() {
analogWrite(LPWM, 0);
analogWrite(RPWM, 0);
delay(5);
}

double mapf(double x, double in_min, double in_max, double out_min, double out_max)
Expand Down
Loading

0 comments on commit 3eeaa62

Please sign in to comment.