SHARE
TWEET

Untitled

a guest Jan 23rd, 2020 67 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1.  
  2. // ----------------------------
  3. // Group number:      6
  4. // Student 1:
  5. // Mattia Strocchi    4802454
  6. // Student 2:
  7. // Pietro Rustici     4888014
  8. // ----------------------------
  9. #include <ros.h>
  10. #include <math.h>
  11. #include <geometry_msgs/Twist.h>
  12.  
  13. // left motor control (reverse, enable, forward)
  14. #define MOTOR_L_REV 7
  15. #define MOTOR_L_EN 24
  16. #define MOTOR_L_FWD 6
  17.  
  18. // right motor control (reverse, enable, forward)
  19. #define MOTOR_R_REV 3
  20. #define MOTOR_R_EN 25
  21. #define MOTOR_R_FWD 2
  22.  
  23. // HC-05
  24. // Arduino Serial1 is already defined on pin 18/19
  25. // The settings of the bluetooth are 57600 baud, 8n1. The pin code for the Bluetooth is 1234 or 0000.
  26.  
  27. // HC-SR04 (distance sensor)
  28. #define TRIG 23
  29. #define ECHO 22
  30. #define MIN_DISTANCE 10
  31. #define MAX_DISTANCE 15
  32.  
  33. // LED
  34. #define LED 13
  35.  
  36. // PWM settings
  37. #define PWM_MIN 60
  38. #define PWM_MAX 90
  39. #define STEERING_FACTOR 35
  40. // 2 seconds delta interval
  41. #define DELTA_INTERVAL 2000
  42.  
  43.  
  44. // ROS SUBSCRIBER AND CB
  45. float leftMotor = 0;
  46. float rightMotor = 0;
  47. float distanceF = 0;
  48. // time
  49. double last_callback = 0;
  50.  
  51. void onNewMessage( const geometry_msgs::Twist& msg){
  52.   float linear = msg.linear.x;
  53.   float angular = msg.angular.z;
  54.   if(msg.linear.x == 0) {
  55.     leftMotor = 0;
  56.     rightMotor = 0;
  57.   } else {
  58.     leftMotor = map(linear, 0, 1, PWM_MIN, PWM_MAX);
  59.     rightMotor = map(linear, 0, 1, PWM_MIN, PWM_MAX);
  60.   }
  61.  
  62.   if(angular != 0){
  63.     leftMotor += STEERING_FACTOR * angular;
  64.     rightMotor -= STEERING_FACTOR * angular;
  65.   }
  66.   // reset callback timer
  67.   last_callback = millis();
  68. }
  69. /*
  70. class NewHardware : public ArduinoHardware {
  71.   public: NewHardware():ArduinoHardware(&Serial1, 57600)
  72.     {};
  73. }; ros::NodeHandle_<NewHardware> node;
  74. */
  75.  
  76. ros::NodeHandle node;
  77.  
  78. geometry_msgs::Twist message;
  79. ros::Subscriber<geometry_msgs::Twist> sub("/cmd_vel", &onNewMessage);
  80. ros::Publisher pub("/sensor_data", &message);
  81.  
  82. void setup() {
  83.   initPins();
  84.   Serial1.begin(57600);
  85.   node.initNode();
  86.   node.advertise(pub);
  87.   node.subscribe(sub);
  88.   // delay ease ros connection process
  89.   delay(1000);
  90.   initInterrupts();
  91. }
  92.  
  93. void loop() {
  94.   // check time elapsed tilll the last callback
  95.   if((millis() - last_callback) > DELTA_INTERVAL) {
  96.     leftMotor = 0;
  97.     rightMotor = 0;
  98.   }
  99.   message.linear.x = leftMotor;
  100.   message.linear.y = rightMotor;
  101.   message.linear.z = distanceF;
  102.   pub.publish(&message);
  103.   node.spinOnce();
  104.   delay(5);
  105. }
  106.  
  107.  
  108. void initPins() {
  109.  
  110.   // init distance sensor
  111.   pinMode(TRIG, OUTPUT);
  112.   pinMode(ECHO, INPUT);
  113.  
  114.   // init all MOTOR pins
  115.   pinMode(MOTOR_L_EN, OUTPUT);
  116.   pinMode(MOTOR_L_REV, OUTPUT);  
  117.   pinMode(MOTOR_L_FWD, OUTPUT);
  118.  
  119.   pinMode(MOTOR_R_EN, OUTPUT);
  120.   pinMode(MOTOR_R_REV, OUTPUT);
  121.   pinMode(MOTOR_R_FWD, OUTPUT);
  122.  
  123.    // enable motors
  124.   digitalWrite(MOTOR_L_EN, HIGH);
  125.   digitalWrite(MOTOR_R_EN, HIGH);
  126.  
  127.   // init led pin
  128.   pinMode(LED, OUTPUT);
  129. }
  130.  
  131. float getDistanceFactor() {
  132.   // Clears the trigPin
  133.   digitalWrite(TRIG, LOW);
  134.   delayMicroseconds(2);
  135.   // Sets the trigPin on HIGH state for 10 micro seconds
  136.   digitalWrite(TRIG, HIGH);
  137.   delayMicroseconds(10);
  138.   digitalWrite(TRIG, LOW);
  139.   // Reads the echoPin, returns the sound wave travel time in microseconds
  140.   long duration = pulseIn(ECHO, HIGH);
  141.   // Calculating the distance
  142.   float distance = duration*0.034/2;
  143.  
  144.   if(distance < MIN_DISTANCE) distance = 0;
  145.   else if (distance > MAX_DISTANCE) distance = 1;
  146.   else {
  147.     distance = (distance - MIN_DISTANCE) / (MAX_DISTANCE - MIN_DISTANCE);
  148.   }
  149.   return distance;
  150. }
  151.  
  152. void initInterrupts() {
  153.   noInterrupts();
  154.   TCCR1A = 0;
  155.   TCCR1B = 0;
  156.   TCNT1  = 0;
  157.   OCR1A = 2500;             // 62500 / 2500 run 25 times a sec
  158.   TCCR1B |= (1 << WGM12);   // CTC mode
  159.   TCCR1B |= (1 << CS12);    // 256 prescaler
  160.   TIMSK1 |= (1 << OCIE1A);  // enable timer compare interrupt
  161.   interrupts();
  162. }
  163.  
  164. ISR(TIMER1_COMPA_vect)          // timer compare interrupt service routine
  165. {
  166.   distanceF = getDistanceFactor();
  167.   analogWrite(MOTOR_L_FWD, rightMotor * distanceF);
  168.   analogWrite(MOTOR_R_FWD, leftMotor * distanceF);
  169.   digitalWrite(LED, digitalRead(LED) ^ 1);
  170. }
  171.  
  172. /*
  173. TCNTx - Timer/Counter Register. The actual timer value is stored here.
  174. OCRx - Output Compare Register
  175. ICRx - Input Capture Register (only for 16bit timer)
  176. TIMSKx - Timer/Counter Interrupt Mask Register. To enable/disable timer interrupts.
  177. TIFRx - Timer/Counter Interrupt Flag Register. Indicates a pending timer interrupt.
  178. Motors use timer 3 (pin 2) and timer 4 (pin 6)
  179. */
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
Top