Diff: robot.cpp

Revision:

4:62a6681510d6

Parent:

3:30bdc3d9e1c6

Child:

5:cf033e9d4468

--- a/robot.cpp	Wed Nov 02 08:11:24 2016 +0000
+++ b/robot.cpp	Wed Nov 23 21:40:26 2016 +0000
@@ -5,128 +5,48 @@
  Version     : 0.1
  Date        : 13-10-2016
  Copyright   : Open for all
- Description : Program to serve af platform for Pro1 2016
+ Description : Program to serve the platform for Pro1 2016
  ============================================================================
  */
-    
-/*
-=============================================================================
-Including the necessary header files
-=============================================================================
-*/
+#include "odometry.h"
 #include "mbed.h"
 #include "HCSR04.h"
 #include "nucleo_servo.h"
 #include "hack_motor.h"
 #include <math.h> 
 
-/*
-=============================================================================
-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
-
-/*
-=============================================================================
-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
-
-//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
-
-//fourth position:
-#define fourth_x 70
-#define fourth_y 50
-  
-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
-// (M1A, M1B, M2A, M2B)
-
-/*
-=============================================================================
-definition for analog power
-=============================================================================
-*/
-AnalogIn   ain(PC_4);
-DigitalOut dout(PB_13);
-DigitalOut greenout(PB_12);
-
-int stop=0;
-/*
-=============================================================================
-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
-
-/*
-=============================================================================
-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
-
+void startfunction( );
 Ticker T1; // create an object T1 of Ticker
 
 /*
 ----------------------------------------------------------------------------
-                            Accessing the int main
+                                MAIN FUNCTION
+----------------------------------------------------------------------------*/
+int main()
+{
+/*
+=============================================================================
+Driving and stopping the robot with member functions from the wheel class
+=============================================================================*/
+    startfunction();    //  1. DRIVE  
+    while(Dtravel_1() <= DISTANCES[0])  //  OVERVEJ AT SMIDE "get_to_goal" funktionen ind i selve while loopet, sådan, at beregningerne sker parallelt. 
+    {
+        get_to_goal(); //   TJEK DETTE
+        robot.FW(0.5, 0.5);
+        wait_ms(5000);
+    }
+    
+    robot.stop(); 
+    printf("\n\rtimeused = %4d tickL= %4d  tickR = %4d \n", t.read_ms(),tickL,
+           tickR);   
+    wait_ms(3000);
+} 
+/*  
 ----------------------------------------------------------------------------
-*/
-int main()
+                            END OF MAIN FUNCTION
+----------------------------------------------------------------------------*/
+
+void startfunction()
 {
     T1.attach(&read_analog, 1.0); // attach the address of the read_analog
     //function to read analog in every second
@@ -145,8 +65,7 @@
 /*
 =============================================================================
 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);
@@ -164,183 +83,4 @@
 
     wait_ms(1000); //wait 1 secs here before you go
     t.start();      // start timer (just demo of how you can use a timer)
-
-    /*  ------------------
-            with PID
-        -----------------
-    --  delete get_to_goal(); when you use without pid
-    get_to_goal ();
-    */
-
-    /* ---------------------------
-           without pid
-       ------------------------
-    */
-    // delete between --- when you use it with pid
-    // ------------------------------------------------------------
-/*
-=============================================================================
-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);    
-           
-} 
-/*  
-----------------------------------------------------------------------------
-                            END OF MAIN FUNCTION
-----------------------------------------------------------------------------
-*/
-
-/*
-=============================================================================
-Calculations of distances traveled by both wheels and robot
-=============================================================================
-*/
-void tickLeft() // udtate left tick on tacho interrupt
-{
-    tickL++;
-}
-void tickRight() // udtate right tick on tacho interrupt
-{
-    tickR++;
-}
-float Dleft() // Distance for left wheel
-{
-    return 2*PI*R*tickL/N;
-}
-
-float Dright() // Distance for right wheel
-{
-    return 2*PI*R*tickR/N;
-}
-
-float Dcenter() // Distance for center
-{
-    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
-{
-    double power = 0;
-    double derivative = 0;
-    double proportional = 0;
-    double integral = 0;
-    double e_old = 0;
-
-    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
-
-        // Compute the integral
-        integral += proportional;
-
-        // Compute the derivative
-        derivative = e - e_old;
-
-        // Remember the last error.
-        e_old = e;
-
-        // Compute the power
-        power = (proportional * (P_TERM)) + (integral * (I_TERM))
-                + (derivative * (D_TERM));
-
-        // Compute new speeds
-
-        right = speed - power;//if power is 0, no error and same speed on wheels
-        left = speed + power;
-
-
-        // limit checks
-        if (right < MIN)
-            right = MIN;
-        else if (right > MAX)
-            right = MAX;
-
-        if (left < MIN)
-            left = MIN;
-        else if (left > MAX)
-            left = MAX;
-
-        robot.FW(right,left); // set new positions
-        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 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
-// could be flagged with global variables like "stop"
-{
-    if(ain > 0.6f) { // 60% power, time for recharge
-        dout = 1;
-        stop=0;
-    } else {
-        dout = not dout;
-        stop=1; // flash red led
-    }
-
-    if (ain==1.0f) greenout = 1; // full power
-    else greenout = 0;
-}
-void init() // init your parameters
-{
-    tickL=0;
-    tickR=0;
 }
\ No newline at end of file