Diff: main.cpp

Revision:

12:0fa4c5a86c75

Parent:

11:35809512ec11

Child:

13:0be39d0abac7

--- a/main.cpp	Wed Nov 13 15:59:06 2019 +0000
+++ b/main.cpp	Fri Nov 15 13:23:56 2019 +0000
@@ -33,52 +33,48 @@
 Sensor sensor_right(I2C_SDA, I2C_SCL, S7);
 
 // Motors
-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);
+Motor motor_left(PC_6, PB_15, PB_13);
+Motor motor_right(PA_15, PC_7, PB_4);
 
 void controlMotor(const std_msgs::Int32& motor_dir)
 {
     switch(motor_dir.data) {
-        // Stop motor
         case 0:
-            motor_left.moveForward(0);
-            motor_right.moveForward(0);
+            motor_left.stop();
+            motor_right.stop();
             break;
             
         // Move forward
         case 1:
-            motor_left.moveForward(5691);
-            motor_right.moveForward(5691);
+            motor_left.moveForward();
+            motor_right.moveForward();
             break;
     
         // Move left
         case 2:
-            motor_left.moveLeft(0.5);
-            motor_right.moveLeft(-0.5);      
+            motor_left.moveBackward();
+            motor_right.moveForward();      
             break;
             
         // Move right
         case 3:
-            motor_left.moveRight(-0.5);
-            motor_right.moveBackward(0.5);
+            motor_left.moveForward();
+            motor_right.moveBackward();
             break;
     }
 }
 
-// ROS subscriber for motors control
-ros::Subscriber<std_msgs::Int32> motor_sub("motor_control", &controlMotor);
-
-
 int main() 
 {
     ros::NodeHandle  nh;
     nh.initNode();
     
     // ROS publisher for sensor readings
-    mbed_custom_msgs::lidar_msg int_lidar_msg;
-    ros::Publisher lidar_pub("lidar_reading", &int_lidar_msg);
+    mbed_custom_msgs::lidar_msg lidar_msg;
+    ros::Publisher lidar_pub("lidar_reading", &lidar_msg);
+    
+    // ROS subscriber for motors control
+    ros::Subscriber<std_msgs::Int32> motor_sub("motor_control", &controlMotor);
 
     nh.advertise(lidar_pub);
     nh.subscribe(motor_sub);
@@ -104,19 +100,18 @@
   
     while (1)
     {        
-        int_lidar_msg.sensor_forward  = sensor_forward.read();
-        int_lidar_msg.sensor_right  = sensor_right.read();
-        int_lidar_msg.sensor_back  = sensor_back.read();
-        int_lidar_msg.sensor_left  = sensor_left.read();         
-
-        lidar_pub.publish(&int_lidar_msg);
+        lidar_msg.sensor_forward = sensor_forward.read();
+        lidar_msg.sensor_back = sensor_back.read();
+        lidar_msg.sensor_left = sensor_left.read();
+        lidar_msg.sensor_right = sensor_right.read();         
+        lidar_pub.publish(&lidar_msg);
         
         /*
         int range;
         range = sensor_forward.read();
         pc.printf("Range = %d\r\n", range);
         */
-        
+    
         nh.spinOnce();
               
         wait_ms(10);