Diff: main.cpp

Revision:

11:35809512ec11

Parent:

10:be9de79cf6b0

Child:

12:0fa4c5a86c75

--- a/main.cpp	Tue Nov 12 13:55:17 2019 +0000
+++ b/main.cpp	Wed Nov 13 15:59:06 2019 +0000
@@ -33,78 +33,43 @@
 Sensor sensor_right(I2C_SDA, I2C_SCL, S7);
 
 // Motors
-Motor motor_left(PC_6, PB_15, PB_13);
-Motor motor_right(PA_15, PC_7, PB_4);
-    
-/*
-void avoidObstacle(const std_msgs::Empty& avoid_obstacle_msg)
-{    
-    // When obstacle ahead
-    if (sensor_forward.getIsObstacle())
-    {
-        if ( (sensor_right.getIsObstacle()) && (sensor_left.getIsObstacle()) )
-        {
-            //Turn backward
-            while(!sensor_back.getIsObstacle())
-            {
-                motor_left.moveForward();
-                motor_right.moveBackward();  
-            }
-        }
-        if (sensor_left.getIsObstacle())
-        {
-            //Turn to the right
-            motor_left.moveForward();
-            motor_right.moveBackward();
-        }
-        else 
-        {
-            // By default : turn to the left
-            motor_left.moveBackward();
-            motor_right.moveForward();      
-        }
-    }
-    // No obstacle
-    else
-    {
-        motor_left.moveForward();
-        motor_right.moveForward();
-    }
-}
-*/
+QEI wheel_A (PB_12, PA_4, NC, 3576, QEI::X4_ENCODING); //counts per rev = 12 * 298:1, use quadrature encoding
+QEI wheel_B (PB_5, PB_3, NC, 3576, QEI::X4_ENCODING); //counts per rev = 12 * 298:1, use quadrature encoding
+Motor motor_left(PC_6, PB_15, PB_13, wheel_A);
+Motor motor_right(PA_15, PC_7, PB_4, wheel_B);
 
-/*
-TODO REMOVE COMMENT WHEN CUSTOM MSGS READY
 void controlMotor(const std_msgs::Int32& motor_dir)
 {
-    switch (motor_dir)
-    {
+    switch(motor_dir.data) {
+        // Stop motor
+        case 0:
+            motor_left.moveForward(0);
+            motor_right.moveForward(0);
+            break;
+            
         // Move forward
         case 1:
-            motor_left.moveForward();
-            motor_right.moveForward();
+            motor_left.moveForward(5691);
+            motor_right.moveForward(5691);
             break;
     
         // Move left
         case 2:
-            motor_left.moveBackward();
-            motor_right.moveForward();      
+            motor_left.moveLeft(0.5);
+            motor_right.moveLeft(-0.5);      
             break;
             
         // Move right
         case 3:
-            motor_left.moveForward();
-            motor_right.moveBackward();
+            motor_left.moveRight(-0.5);
+            motor_right.moveBackward(0.5);
             break;
     }
 }
-*/
 
-/*
-TODO REMOVE COMMENT WHEN CUSTOM MSGS READY
 // ROS subscriber for motors control
 ros::Subscriber<std_msgs::Int32> motor_sub("motor_control", &controlMotor);
-*/
+
 
 int main() 
 {
@@ -114,16 +79,9 @@
     // ROS publisher for sensor readings
     mbed_custom_msgs::lidar_msg int_lidar_msg;
     ros::Publisher lidar_pub("lidar_reading", &int_lidar_msg);
-    /*
-    std_msgs::Int32 int_msg;
-    ros::Publisher lidar_pub("lidar_reading", &int_msg);
-    */
+
     nh.advertise(lidar_pub);
-    
-    /*
-    TODO REMOVE COMMENT WHEN CUSTOM MSGS READY
     nh.subscribe(motor_sub);
-    */
 
     // load settings onto VL6180X sensors 
     sensor_forward.init();
@@ -152,11 +110,6 @@
         int_lidar_msg.sensor_left  = sensor_left.read();         
 
         lidar_pub.publish(&int_lidar_msg);
-
-        /*
-        int_msg.data = sensor_forward.read();
-        lidar_pub.publish(&int_msg);
-        */
         
         /*
         int range;