Diff: main.cpp

Revision:

16:11282b7ee726

Parent:

15:7c9bf41dd187

Child:

17:8831c676778b

--- a/main.cpp	Tue Nov 19 15:28:02 2019 +0000
+++ b/main.cpp	Sat Nov 23 15:33:23 2019 +0000
@@ -12,7 +12,9 @@
 // Set up serial to pc
 //Serial pc(SERIAL_TX, SERIAL_RX); 
 
+//Threading for sensor readings
 Thread thread1;
+Thread thread2;
 Mutex SerialMutex;
         
 // Set up I²C on the STM32 NUCLEO-401RE 
@@ -30,20 +32,20 @@
 #define S7 PG_2
 #define S8 PG_3 
 
-/*Ticker Sensor_readings()
-rangeForward = sensor_forward.read();
-*/
-
 // VL6180x sensors
 Sensor sensor_back(I2C_SDA, I2C_SCL, S1);
 Sensor sensor_left(I2C_SDA, I2C_SCL, S3);
 Sensor sensor_forward(I2C_SDA, I2C_SCL, S5);
 Sensor sensor_right(I2C_SDA, I2C_SCL, S7);
 
+// Floats for sensor readings
 float sensor_forward_reading;
 float sensor_back_reading;
 float sensor_left_reading;
 float sensor_right_reading;
+double speed=0.6;
+bool control_type = 1;;
+int32_t motor_option;
 
 // Set motorpins for driving
 Motor A(PB_13, PB_15, PC_6, 1); // pwm, fwd, rev, can brake
@@ -51,6 +53,7 @@
 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
 
+//Thread function to update sensor readings
 void flip() {
         while(1) {
             SerialMutex.lock();
@@ -64,68 +67,110 @@
 
 }
 
+// Function to move robot (motor speed, motor speed, pulses to travel)
 void move(float motorA, float motorB, int distance)
 {
-    // Variables to allow for any pulse reading
     float current_pulse_reading = 0;
     
     // - is forward on our robot
     A.speed(motorA);
     B.speed(motorB);
-    // loop for moving forward
+    
         while(abs(current_pulse_reading) <= distance)
         {
-            //pc.printf("current_pulse_reading 1= %f\r\n", current_pulse_reading);
             current_pulse_reading = wheel_A.getPulses();
-            //SerialMutex.lock();
-            //pc.printf("forward = %f, back = %f\n\r", sensor_forward_reading, sensor_back_reading);
-            //SerialMutex.unlock();
+            //Stops Robot moving forward if front sensor detects
             if (sensor_back_reading <255 and motorA>0 and motorB>0){
                 return;
                 }
+            //Stops robot reversing if back sensors detects
             if (sensor_forward_reading <255 and motorA<0 and motorB<0){
                 return;
                 }
         }
-        
-//    A.speed(0);
-//    B.speed(0);
-//    
-//    wheel_B.reset();
-//    wheel_A.reset();
-
 }
 
 
-void controlMotor(const std_msgs::Int32& motor_dir)
+void motor_callback(const std_msgs::Int32& motor_dir)
 {
-    switch(motor_dir.data) {
- 
-        case 0://49
-            move(-0.5, -0.5, 10000); 
-            break;
-    
-        // Move left
-        case 1://50
-            move(0.5, -0.5, 5000); 
-            break;
+    motor_option = motor_dir.data;
+    thread2.signal_set(1);
+}
+
+void control_motor(void) {
+    while (1)
+        {   
+            thread2.signal_wait(1);
+            switch(motor_option) {
+                //Forward 30cm
+                case 0://49
+                    if (control_type == 1){
+                        move(-speed, -speed, 5960);
+                        }
+                    else if (control_type ==0){
+                        move(-speed, -speed, 50);                    
+                        } 
+                    break;
             
-        // Move right
-        case 2://51
-            move(-0.5, 0.5, 5000); 
-            break;
+                //Left 30 degrees
+                case 1://50
+                    if (control_type == 1 ){
+                        move(speed, -speed, 894);
+                        }
+                    else if (control_type == 0){
+                        move(speed, -speed, 50);
+                        } 
+                    break;
+                    
+                //Right 30 degrees
+                case 2://51
+                    if (control_type == 1 ){
+                        move(-speed, speed, 894);
+                        }
+                    else if (control_type == 0 ){
+                        move(-speed, speed, 30);
+                        }     
+                    break;
+                    
+                // Reverse  30cm
+                case 3://52
+                    if (control_type ==1 ){
+                        move(speed, speed, 5960);
+                        }
+                    else if (control_type ==0 ){
+                        move(speed, speed, 50); 
+                        }
+                    break;
+                
+                case 4://speed up
+                    if (speed<1.0){
+                        speed += 0.2;
+                        }
+                    break;
+                    
+                case 5://speed down
+                    if (speed>0.2){
+                        speed -= 0.2;
+                        }
+                    break;
+    
+                case 6: // switch control type
+                    control_type =! control_type;
+                    break;
+                }
+    
+                /*
+                case 8: //control type 3
+                    control_type = 3;
+                */
+            //Reset speed and pulse count
+            A.speed(0);
+            B.speed(0);
             
-        // Reverse
-        case 3://52
-            move(0.5, 0.5, 10000);
-            break;
-            
-            }
-    A.speed(0);
-    B.speed(0);
-    
-    wheel_B.reset();
-    wheel_A.reset();
+            wheel_B.reset();
+            wheel_A.reset();
+            //nh.spinOnce();
+    }
 }
 
 int main() 
@@ -134,7 +179,7 @@
     nh.initNode();
     
     // ROS subscriber for motors control
-    ros::Subscriber<std_msgs::Int32> motor_sub("motor_control", &controlMotor);
+    ros::Subscriber<std_msgs::Int32> motor_sub("motor_control", &motor_callback);
 
     nh.subscribe(motor_sub);
     
@@ -154,13 +199,14 @@
     sensor_left.init();
     
     thread1.start(callback(&flip));
+    thread2.start(callback(&control_motor));
     
-    while (1)
-    {   
+    while(1) {
         nh.spinOnce();
-        wait(1);
+        
+    }
   
-    }
+    
 }