Diff: robot.cpp

Revision:

2:1c27a43bb9b7

Parent:

1:396a582e8861

--- a/robot.cpp	Mon Oct 31 10:00:46 2016 +0000
+++ b/robot.cpp	Wed Nov 02 08:05:41 2016 +0000
@@ -1,101 +1,140 @@
 /*
  ============================================================================
  Name        : robot.cpp
- Author      : Henning Slavensky
+ Author      : Team 5
  Version     : 0.1
- Date        : 13102016
+ Date        : 13-10-2016
  Copyright   : Open for all
  Description : Program to serve af platform for Pro1 2016
  ============================================================================
  */
+    
+/*
+=============================================================================
+Including the necessary header files
+=============================================================================
+*/
 #include "mbed.h"
 #include "HCSR04.h"
 #include "nucleo_servo.h"
 #include "hack_motor.h"
-#include <math.h> // nice when you got to work with real geometry stuff
-
-#define PI 3.141592 // definition of PI
+#include <math.h> 
 
-// PID definitions
-#define P_TERM 0.1
-#define I_TERM 0
-#define D_TERM 0.0
-#define MAX 1.0
+/*
+=============================================================================
+PID definitions
+=============================================================================
+*/
+#define P_TERM 0.2
+#define I_TERM 0.1
+#define D_TERM 0.3
+#define MAX 1.0 
 #define MIN 0
-
+#define PI 3.141592
 
-/* ------------------------------
-    Definitions for robot
-    note that N,R,L maybe not have the most meaningful name
-    but they follow the names from the theory for our
-    robot lectures, and can be used in youa application
-   ------------------------------
-*/
-                
+/*
+=============================================================================
+Geometry for robot:
+note that N,R,L maybe not have the most meaningful name
+but they follow the names from the theory for our
+robot lectures, and can be used in youa application
+=============================================================================
+*/         
 #define N 20 // ticks on wheel
 #define R 32.5 // radius = 32.5 mm
 #define L 133 // 133 mm distance between wheels
+ 
+/*
+=============================================================================
+Coordinates for robot to reach
+=============================================================================
+*/
+//Start position:
+#define first_x 0
+#define first_y 0
+#define first_angle 0.785  //45 deg
 
-//Coordinates for Robot with x and y
-#define starting_x = 0
-#define starting_y = 0 
-#define second_x = 5
-#define second_y = 4
-#define third_x = 7
-#define third_y = 1
+//Second position:
+#define second_x 50
+#define second_y 40
+#define second_angle 1.047 //60 deg
+
+//Third position:
+#define third_x 70
+#define third_y 10
+#define third_angle 1.396 //80 deg
 
-double distcalc(double, double)
-{
-    
-}
-
+//fourth position:
+#define fourth_x 70
+#define fourth_y 50
+/*
+=============================================================================
+Calculation of length between points
+=============================================================================
+*/
+  double angle_desire = (second_y-first_y)/(second_x-first_x); 
+  double error_present = atan(angle_desire)-0.785; 
+  
 Timer t; // define a timer t
 Serial pc(USBTX, USBRX); // not used here because we only have one serial
 // connection
 InterruptIn tacho_left(PC_3); // Set PC_3 to be interupt in for tacho left
 InterruptIn tacho_right(PC_2);// Set PC_2 to be interupt in for tacho right
 
-Wheel robot(PB_2, PB_1, PB_15, PB_14);  // create an object of robot H-bridge
+Wheel robot(PB_2, PB_1, PB_15, PB_14);  
+// create an object of robot H-bridge
 // (M1A, M1B, M2A, M2B)
 
-/*  --------------------------- 
-    definition for analog power
-    ---------------------------
+/*
+=============================================================================
+definition for analog power
+=============================================================================
 */
 AnalogIn   ain(PC_4);
 DigitalOut dout(PB_13);
 DigitalOut greenout(PB_12);
 
 int stop=0;
-
-/*  ---------------------
-        Global variables
-    --------------------
-    */
+/*
+=============================================================================
+Global variables
+=============================================================================
+*/    
 double right,left;
 double speed=0.5;
 double e = 0; // angle error
 int tickL = 0; // tick on left wheel
 int tickR = 0; // tick on right wheel
 
-/*  -----------
-    Prototypes
-    -----------
-*/
+/*
+=============================================================================
+Prototype defined functions
+=============================================================================
+*/ 
 void init();
 void tickLeft(); // read tick left
 void tickRight(); // read tick right
 float Dleft(); // distance left wheel
 float Dright(); // distance right wheel
 float Dcenter(); // Distance for center
+
+//functions for calculating desired distances from defined points
+float Dist_reach1();
+float Dist_reach2();
+float Dist_reach3();
+
 void get_to_goal ( ); // function to get to goal
 void read_analog(); // comes here every second
 
 Ticker T1; // create an object T1 of Ticker
 
+/*
+----------------------------------------------------------------------------
+                            Accessing the int main
+----------------------------------------------------------------------------
+*/
 int main()
 {
-
     T1.attach(&read_analog, 1.0); // attach the address of the read_analog
     //function to read analog in every second
 
@@ -110,22 +149,23 @@
 
     sensor.setRanges(2, 400); // set the range for Ultrasonic
 
-    /*  -----------------------------------------
-        Demo of the servor and ulta sonic sensor
-        -----------------------------------------
-    */
+/*
+=============================================================================
+Demo of the servor and ulta sonic sensor
+=============================================================================
+*/  
     wait_ms(2000); // just get time for you to enable your screeen
     servo1.set_position(0); // Servo right position (angle = 0 degree) for servo
     wait_ms (500);
-    printf("\r\nDistance: %5.1f mm", sensor.getDistance_mm()); // display the
+    printf("\r\nDistance: %5.1f mm \n", sensor.getDistance_mm()); // display the
     //readings from ultra sonic at this position
     servo1.set_position(180); // Servo left position (angle = 180 degree)
     //for servo
     wait_ms (500);
-    printf("\r\nDistance: %5.1f mm", sensor.getDistance_mm());
+    printf("\r\nDistance: %5.1f mm \n", sensor.getDistance_mm());
     servo1.set_position(90); // Straight ahead (angle = 90 degree) for servo
     wait_ms (500);
-    printf("\r\nDistance: %5.1f mm", sensor.getDistance_mm());
+    printf("\r\nDistance: %5.1f mm \n", sensor.getDistance_mm());
 
     init(); // initialise parameters (just for you to remember if you need to)
 
@@ -145,20 +185,32 @@
     */
     // delete between --- when you use it with pid
     // ------------------------------------------------------------
-    while (Dcenter() <= CLOSE_ENOUGH) { // while distance traveled is less than
-        //target
-        
-            robot.FW(0.5,0.5); // set new values half speed on both wheels
-            wait_ms(20);
+/*
+=============================================================================
+Driving and stopping the robot with member functions from the wheel class
+=============================================================================
+*/
+    while(Dcenter() <= Dist_reach1())
+    {
+        robot.FW(0.5, 0.5);
+        wait_ms(5000);
     }
-    // --------------------------------------------------------------
-
     robot.stop(); // stop the robot again
     printf("\n\rtimeused = %4d tickL= %4d  tickR = %4d ", t.read_ms(),tickL,
-           tickR);
+           tickR);    
+           
+} 
+/*  
+----------------------------------------------------------------------------
+                            END OF MAIN FUNCTION
+----------------------------------------------------------------------------
+*/
 
-}
-
+/*
+=============================================================================
+Calculations of distances traveled by both wheels and robot
+=============================================================================
+*/
 void tickLeft() // udtate left tick on tacho interrupt
 {
     tickL++;
@@ -169,13 +221,11 @@
 }
 float Dleft() // Distance for left wheel
 {
-
     return 2*PI*R*tickL/N;
 }
 
 float Dright() // Distance for right wheel
 {
-
     return 2*PI*R*tickR/N;
 }
 
@@ -184,25 +234,60 @@
     return (Dleft()+Dright())/2;
 }
 
+//Calculation of current position and new position with angles!!!
 
+float angle_new()
+{
+    return (Dright()-Dleft())/L;   
+}
+
+float x_position()
+{
+    return Dcenter()*sin(angle_new());   
+}
+
+float y_position()
+{
+    return Dcenter()*cos(angle_new());
+}
+
+//slet efterfølgende
+float Dist_reach1() 
+{ 
+  double firstTosecond_calc = ((second_x-first_x)^2)+((second_y-first_y)^2);
+  double dist_firstTosecond = sqrt(firstTosecond_calc); 
+  
+  return (dist_firstTosecond);
+}
+
+/*
+=============================================================================
+PID init
+=============================================================================
+*/
 void get_to_goal ( ) // function to get to goal
 {
-
-    // PID init
-    double power, derivative, proportional, integral, e_old;
-    power=derivative=proportional=integral=e_old=0;
-
-    while (Dcenter() < CLOSE_ENOUGH) {
+    double power = 0;
+    double derivative = 0;
+    double proportional = 0;
+    double integral = 0;
+    double e_old = 0;
 
-        e = tickR-tickL; // error is difference between right tick and left tick
-        // should be ajusted to your context, angle for robot
-        // and instead of test Dcenter() in your while loop, test
-        // for distance_to_goal()
-
-        /* --------------------------
-         * PID regulator
-         * --------------------------
-         */
+    while (Dcenter() < Dist_reach1()) {
+/*
+-------------------------------------------------
+error is difference between right tick and left tick
+should be ajusted to your context, angle for robot
+and instead of test Dcenter() in your while loop, test
+for distance_to_goal()
+-------------------------------------------------
+*/
+        e = tickR-tickL; 
+/*
+=============================================================================
+PID regulator
+=============================================================================
+*/
 
         // Compute the proportional
         proportional = e; // get the error
@@ -241,13 +326,10 @@
         printf("\n\r Dcenter = %3.2f tickL= %4d  tickR = %4d left %3.2f right %3.2f",
                Dcenter(),tickL,tickR,left,right);
 
-        wait_ms(20); // should be adusted to your context. Give the motor time
+        wait_ms(20); // should be adjusted to your context. Give the motor time
         // to do something before you react
-
     }
-
     t.stop(); // stop timer
-
 }
 
 void read_analog() // comes here every second in this case