Revision 11:12e73437dc9f, committed 2020-01-10

Comitter:

hongyunAHN

Date:

Fri Jan 10 15:05:11 2020 +0000

Parent:

10:51dd168b5a96

Commit message:

ii

Changed in this revision

--- a/Motors/Motor.cpp	Mon Jan 06 20:38:07 2020 +0000
+++ b/Motors/Motor.cpp	Fri Jan 10 15:05:11 2020 +0000
@@ -1,4 +1,4 @@
-/*--------------------------------------------------------------------------------
+ /*--------------------------------------------------------------------------------
 Filename: main.cpp
 Description: Main program loop
 --------------------------------------------------------------------------------*/
@@ -9,7 +9,7 @@
 #include <ros.h>
 #include <Servo.h>
 #include <std_msgs/UInt8.h>
-//#include <std_msgs/UInt16.h>
+
 
 
 cMotor MotorL(M1_PWM, M1_IN1, M1_IN2);  //Left motor class and pin initialisation
@@ -17,8 +17,7 @@
 
 float *TOFrangePtr[4];
 
-//Servo servo1(srvoTilt);
-//Servo servo2(srvoPan);
+
 
 //Class definitions
 cBuzzer cBuzzer(Buzz);
@@ -38,6 +37,9 @@
     printf("%d", keyPress.data);
 
     // Lowercase chars //
+    //  get keypress information at rasbarry pi to move the DC motor. 
+    //"w,W" move forward, "a,A" move left side, "d,D" move right side,"s,S" move backward  
+    // "q" stop the motor, "e" check the obstacles automatically  
     if (keyPress.data == 119) { // w
         if (keyPress.data  == 88 || 120) { // X
             MotorR.Stop();
@@ -125,7 +127,6 @@
         MotorR.Stop();
         MotorL.Stop();
    }
-       //Stop command
     else if (keyPress.data == 'e') {
         Check_for_obstacles(*TOFrangePtr);
    }
@@ -153,6 +154,10 @@
 Output Parameters: N/A
 Description: Simple obstacle avoidance functionality
 ----------------------------------------------------------------------------------*/
+// using the 4 sensor to check the obstacles and escape the obstacles. 
+//  TOFRange[0]is back sensor,  TOFRange[1] is leftside sensor, TOFRange[2] is front sensor and  TOFRange[4] is rightside sensor 
+// devide the front sensor range to 200~150 and 150~0. 
+// when robot detect the obstacles the buzzer and blue led are ON.  
 void Check_for_obstacles(float TOFRange[4])
 {
     //If top right sensor(6) detects something
@@ -303,7 +308,8 @@
 39(right arrow)
 40(down arrow)
 */
-
+// get the keypress data in rasbarry pi and  move  the 2 servo motor. 
+//"j" move servo1 to left arrow, "l" move servo1 to right arrow, "i" move servo2 to up arrow, "k" move servo2 to down arrow.  
 void servoKeySub(const std_msgs::UInt8 &ServoKeyPress)
 {
     printf("%d", ServoKeyPress.data);

--- a/main.cpp	Mon Jan 06 20:38:07 2020 +0000
+++ b/main.cpp	Fri Jan 10 15:05:11 2020 +0000
@@ -33,14 +33,6 @@
     mySTM32(): MbedHardware(PD_5, PD_6, 57600) {};
 };
 
-/*
-void servo1_cb( const std_msgs::UInt16& cmd_msg) {
-    servo1.position(cmd_msg.data); //set servo angle, should be from 0-180
-}
-void servo2_cb( const std_msgs::UInt16& cmd_msg) {
-    servo2.position(cmd_msg.data); //set servo angle, should be from 0-180
-}    
-*/
 
 ros::NodeHandle_<mySTM32> nh;
 
@@ -51,22 +43,7 @@
 std_msgs::UInt8MultiArray range_msg;
 ros::Publisher TOFreadings("TOFreadings", &range_msg);
 
-//std_msgs::UInt16MultiArray range_msg;
-//ros::Publisher TOFstuff("TOFstuff", &range_msg);
 
-/*sensor_msgs::Range range_msg1, range_msg2,range_msg3,range_msg4;
-ros::Publisher tof1("tof1", &range_msg1);
-ros::Publisher tof2("tof2", &range_msg2);
-ros::Publisher tof3("tof3", &range_msg3);
-ros::Publisher tof4("tof4", &range_msg4);*/
-
-//std_msgs::Int32MultiArray range_msg;
-//array.data.clear();
-
-
-/* NOT USED ANYMORE */
-//ros::Subscriber<std_msgs::UInt16> sub2("servo1", servo1_cb);
-//ros::Subscriber<std_msgs::UInt16> sub3("servo2", servo2_cb);
 
 
 DigitalIn user_button(USER_BUTTON);
@@ -95,19 +72,13 @@
 
 
 std_msgs::UInt8MultiArray m;
-//sensor_msgs::Range range_msg[4];
+
 
  
 float TOFRange[4];
  
 DigitalOut led = LED1;
-/*
-Timer t;  // Timer 
 
- char frameid1[] = "/Rear Sensor";
- char frameid2[] = "/Front Left Sensor";
- char frameid3[] = "/Front Center Sensor"; 
- char frameid4[] = "/Front Right Sensor";*/
  
 /* Private Functions----------------------------------------------------------*/ 
 void power_check(void);
@@ -118,19 +89,13 @@
 -------------------------------------------------------------------------------*/
 int main()
 {
-    //t.start(); 
-    //IncSize=1;
+    
     
     nh.initNode();
     nh.subscribe(sub);
     nh.subscribe(sub1);
-    //nh.subscribe(sub2);
-    
-    /*nh.advertise(tof1);
-    nh.advertise(tof2);
-    nh.advertise(tof3);
-    nh.advertise(tof4);*/
-    
+
+  
     //Publishers
     nh.advertise(TOFreadings); //TOF readings
     
@@ -161,7 +126,7 @@
         TOFRange[1] = serviceTOF(VL6180X, ADDR4);
         TOFRange[2] = serviceTOF(VL6180X, ADDR5);
         TOFRange[3] = serviceTOF(VL6180X, ADDR6);
-
+        // 
         TOFrangePtr[0] = &TOFRange[0];
         TOFrangePtr[1] = &TOFRange[1];
         TOFrangePtr[2] = &TOFRange[2];
@@ -175,25 +140,10 @@
         
         //Publish data
         TOFreadings.publish(&range_msg);
-       /* //Check_for_obstacles(TOFRange);
-       range_msg1.header.frame_id =frameid1;
-       range_msg2.header.frame_id =frameid2;
-       range_msg3.header.frame_id =frameid3;
-       range_msg4.header.frame_id =frameid4;
-       
-       range_msg1.range = TOFRange[0];
-       range_msg2.range = TOFRange[1];
-       range_msg3.range = TOFRange[2];
-       range_msg4.range = TOFRange[3];
-        tof1.publish(&range_msg1);
-        tof2.publish(&range_msg2);
-        tof3.publish(&range_msg3);
-        tof4.publish(&range_msg4);*/
-        
         // Need to write code like the obstical ovaoidance, to stop the motors, by the nucleo 
         // sending a char to the PI
         
-        //pc.printf("Spinning...");
+       
         nh.spinOnce();
         wait_ms(5);
     }