diff --git a/callbacks_clean.ino b/callbacks_clean.ino deleted file mode 100644 index a2381b4..0000000 --- a/callbacks_clean.ino +++ /dev/null @@ -1,34 +0,0 @@ -/* - *@infoROS callbacks are automatically linked with through the Arduino IDE - */ -void ThrottlePIDArrayCallback(const std_msgs::Float32MultiArray & array) { - /* - *P,I,D respectively - * for(int i = 0; i < 3; i++) - * { - * //array[i] = throttle_PID_val[i]; - * } - * ThrottlePID.SetTunings(throttle_PID_val[0], throttle_PID_val[1], throttle_PID_val[2]); -} - */ - - -void cmdVelocityCallback(const std_msgs::Float32 & cmd_vel_msg) -{ - // goal_velocity = cmd_vel_msg.data; - autonomous_throttle = cmd_vel_msg.data > 0 ? cmd_vel_msg.data* 21.5 + 1530: 1500; - autonomous_throttle = constrain(autonomous_throttle, 1300, 1650); - velDebug.data = autonomous_throttle; - velDebugger.publish(&velDebug); -} - -void cmdSteeringCallback(const std_msgs::Float32 & cmd_str_msg) -{ - /* - *limits steering input from -30 to 30 degrees - */ - steeringAngle = ((((-1*cmd_str_msg.data)+ 0.5236)/(2* 0.5236))*(2000-1000))+ 960; - steeringAngle = constrain(steeringAngle,980,2000); - debug.data = steeringAngle; - debugger.publish(&debug); -} diff --git a/rr_arduino/imu.ino b/rr_arduino/imu.ino index b6872f3..7a045af 100644 --- a/rr_arduino/imu.ino +++ b/rr_arduino/imu.ino @@ -38,6 +38,5 @@ void ImuReadings() magnetic_msg.magnetic_field.x=mag.x(); magnetic_msg.magnetic_field.y=mag.y(); magnetic_msg.magnetic_field.z=mag.z(); - magnetic_pub(&magnetic_msg); - + magnetic_pub.publish(&magnetic_msg); } diff --git a/rr_arduino/robot_racer/robot_racer.cpp b/rr_arduino/robot_racer/robot_racer.cpp index 87f412c..8c8507b 100644 --- a/rr_arduino/robot_racer/robot_racer.cpp +++ b/rr_arduino/robot_racer/robot_racer.cpp @@ -4,6 +4,7 @@ */ #include "robot_racer.h" +#include /* * @brief Constructs Car and assigns values to attributes @@ -23,6 +24,11 @@ Car::Car() forward_throttle_multiplier_ = (MAX_RC_VAL - REST_RC_VAL) / (float)(MANUAL_MAX - NEUTRAL); left_steering_multiplier_ = (MAX_RC_STEER_VAL - REST_STEER_VAL) / (float)(MAX_STEERING - STEER_NEUTRAL); right_steering_multiplier_ = (MIN_RC_STEER_VAL - REST_STEER_VAL) / (float)(MIN_STEERING - STEER_NEUTRAL); + x_ = 0.0; + y_ = 0.0; + theta_ = 0.0; + odom_trans_.header.frame_id = "odom"; + odom_trans_.child_frame_id = "base_link"; } /* *@brief assigns pin values @@ -280,6 +286,55 @@ void Car::WriteToServos() void Car::SetPreviousTime(long time){ previous_ = time; } + long Car::GetPreviousTime(){ return previous_; } + +void Car::GetOdomTrans() { + return odom_trans_; +} + +void Car::GetOdomMsg() { + return odom_; +} + +void Car::RawToOdom(float vel, float str_angle) { + // Length of the car is 0.335 m + double L = 0.335; + long current_time = millis(); + long time_diff = current_time - GetPreviousTime(); + // If the robot is at the origin, calculate the position using steering angle and the velocity + if (str_angle != 0.0 && x_ == 0.0 && y_ == 0.0) { + // store str_angle in radians + theta_ = str_angle * M_PI / 180.0; + x_ = vel * cos(theta_) * time_diff; + y_ = vel * sin(theta_) * time_diff; + + } else { + // Kinematic equations used: https://nabinsharma.wordpress.com/2014/01/02/kinematics-of-a-robot-bicycle-model/ + // Calculate turning angle beta + double d = vel * time_diff; + double R = L / tan(str_angle); + double beta = d / R; + double xc = x_ - R * sin(theta_); + double yc = y_ + R * cos(theta_); + + x_ = xc + R * sin(theta_ + beta); + y_ = yc - R * cos(theta_ + beta); + theta_ = fmod((theta_ + beta),(2 * M_PI)); + } + + geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(theta_); + + odom_trans_.header.stamp = current_time; + odom_trams_.transform.translation.x = x_; + odom_trams_.transform.translation.y = y_; + odom_trams_.transform.translation.z = 0.0; + odom_trams_.transform.rotation = odom_quat; + + odom_.pose.pose.position.x = x_; + odom_.pose.pose.position.y = y_; + odom_.pose.pose.position.z = 0.0; + odom_.pose.pose.orientation = odom_quat; +} diff --git a/rr_arduino/robot_racer/robot_racer.h b/rr_arduino/robot_racer/robot_racer.h index aa81807..89811f4 100644 --- a/rr_arduino/robot_racer/robot_racer.h +++ b/rr_arduino/robot_racer/robot_racer.h @@ -3,6 +3,9 @@ #include "Servo.h" #include "Arduino.h" +#include +#include +#include // #define BRAKE #define TEST_OUTPUT 1 @@ -45,7 +48,7 @@ class Car { private: int RC_signal_[RC_CHANNELS]; - CarState car_state_; + CarState car_state_; unsigned int steering_auto_; unsigned int throttle_rc_; unsigned int steering_rc_; @@ -59,7 +62,13 @@ class Car float left_steering_multiplier_; float right_steering_multiplier_; long previous_; - + float x_; + float y_; + float theta_; + geometry_msgs::TransformStamped odom_trans_; + nav_msgs::Odometry odom_; + tf::TransformBroadcaster odom_broadcaster_; + Servo ThrottleServo_, SteerServo_; #ifdef BRAKE Servo BrakeServo_;//< brakeServo not used? @@ -78,5 +87,8 @@ class Car void WriteToServos(); long GetPreviousTime(); void SetPreviousTime(long time); + void GetOdomTrans(); + void GetOdomMsg(); + void RawToOdom(float vel, float str_angle); }; #endif diff --git a/rr_arduino/rr_arduino.ino b/rr_arduino/rr_arduino.ino index 5a1048d..05df608 100644 --- a/rr_arduino/rr_arduino.ino +++ b/rr_arduino/rr_arduino.ino @@ -1,12 +1,12 @@ /* - * @file Robot Racer Controller + * @file Robot Racer Controller * @author Tom Meredith - * @author Noah Abradjian - * @author Toni Ogunmade - * @author Abhi Srikantharajah + * @author Noah Abradjian + * @author Toni Ogunmade + * @author Abhi Srikantharajah * @author Brian Kibazohi * @competition IARRC 2018 - * Last Updated: June 24, 2018 + * Last Updated: June 24, 2018 */ //#define TEST_OUTPUT @@ -28,13 +28,11 @@ #include #include - //Serial, velocity and battery monitoring defines respectively const float ROS_BAUD_RATE = 57600; -const float IMU_BAUD_RATE = 9600; const float RC_BAUD_RATE = 115200; -//I2C address for encoder counter +//I2C address for encoder counter const int SLAVE_ADDRESS = 07; Adafruit_BNO055 bno = Adafruit_BNO055(55); @@ -47,7 +45,7 @@ const int AVERAGING_SIZE = 5; /** *@brief function Call *publishes actual velocity & average battery value - *@param current_time + *@param current_time *@returns void */ void GetBatteryState(long current_time); @@ -65,7 +63,7 @@ double rr_velocity = 0.0f , goal_velocity = 0.0f, autonomous_throttle = 1500.0f; //brief ROS communication setup begins //node initialization - + ros::NodeHandle nh; // message objects created @@ -86,7 +84,6 @@ void CmdVelocityCallback(const std_msgs::Float32 &cmd_vel_msg); void CmdSteeringCallback(const std_msgs::Float32 &cmd_str_msg); - // ROS publisher and subscriber commands ros::Publisher state_pub("/arduino/vehicle_state", &state_msg); @@ -99,7 +96,6 @@ ros::Publisher magnetic_pub("/arduino/mag_data", &magnetic_msg); ros::Subscriber velocity_sub ("/PathPlanner/vel_level", CmdVelocityCallback); ros::Subscriber steering_sub ("/PathPlanner/steer_cmd", CmdSteeringCallback); - int steering_angle = 1500; int ROS_watchdog = 0; @@ -114,25 +110,12 @@ void setup() { #ifdef TEST_OUTPUT Serial.begin(ROS_BAUD_RATE); #endif - Serial.begin(IMU_BAUD_RATE); Serial2.begin(RC_BAUD_RATE); - //Initialise the BNO055 sensor - if(!bno.begin()) - { - //Detecting BNOO55 - Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!"); - while(1); - } - - delay(1000); - - bno.setExtCrystalUse(true); /* *ROS Node Handler setup *@param specified topics */ - nh.getHardware()->setBaud(ROS_BAUD_RATE); nh.initNode(); nh.advertise(state_pub); nh.advertise(encoder_pub); @@ -144,7 +127,6 @@ void setup() { nh.subscribe(velocity_sub); nh.subscribe(steering_sub); - robot_racer.setup(); /** *PID setup @@ -153,7 +135,18 @@ void setup() { *ThrottlePID.SetSampleTime(20); // acc = bno.getVector(Adafruit_BNO055::VECTOR_LINEARACCEL); - imu::Vector<3> gyro = bno.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE); - imu::Vector<3> euler = bno.getVector(Adafruit_BNO055::VECTOR_EULER); - imu::Vector<3> mag = bno.getVector(Adafruit_BNO055::VECTOR_MAGNETOMETER); - -/* Display the floating point data */ - Serial.print("XA: "); - Serial.print(acc.x()); - Serial.print(" YA: "); - Serial.print(acc.y()); - Serial.print(" ZA: "); - Serial.print(acc.z()); - Serial.println("\n"); - - -/* Display the floating point data */ - Serial.print(" GX: "); - Serial.print(gyro.x()); - Serial.print(" GY: "); - Serial.print(gyro.y()); - Serial.print(" GZ: "); - Serial.print(gyro.z()); - Serial.println("\n"); - - - /* Display the floating point data */ - Serial.print(" X: "); - Serial.print(euler.x()); - Serial.print("\tY: "); - Serial.print(euler.y()); - Serial.print("\tZ: "); - Serial.print(euler.z()); - Serial.print("\n"); - - - Serial.print(" MX: "); - Serial.print(mag.x()); - Serial.print(" MY: "); - Serial.print(mag.y()); - Serial.print(" MZ: "); - Serial.print(mag.z()); - Serial.println("\n"); - - - - - //remove comment on delay to read data on serial monitor - //delay(1000); + //call function to publish imu_readings ImuReadings(); } @@ -263,7 +210,7 @@ void GetBatteryState(long current_time){ int battery_percentage = 100 * battery_value / 1024; //Set up array for low pass averaging at first time - + static int percentages[AVERAGING_SIZE]; if (prev_time == 0){ for (int i = 0; i < AVERAGING_SIZE; i++){ @@ -272,7 +219,7 @@ void GetBatteryState(long current_time){ } // update array, calculate sum - + int sum = 0; for (int i = AVERAGING_SIZE - 1; i > 0; --i){ percentages[i] = percentages[i - 1]; diff --git a/rr_traffic_light/launch/traffic_light_camera.launch b/rr_traffic_light/launch/traffic_light_camera.launch index efe911e..8dece7a 100644 --- a/rr_traffic_light/launch/traffic_light_camera.launch +++ b/rr_traffic_light/launch/traffic_light_camera.launch @@ -6,7 +6,9 @@ - + + +