Revision 7:a852e63cac3d, committed 2016-12-09

Comitter:

kimnielsen

Date:

Fri Dec 09 12:32:21 2016 +0000

Parent:

6:c2d4a02115db

Commit message:

Kildekode til robot

Changed in this revision

diff -r c2d4a02115db -r a852e63cac3d HCSR04.cpp
--- a/HCSR04.cpp	Thu Nov 24 09:15:48 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,70 +0,0 @@
-/*
-Dette er cpp filen for ultralydssensoren
-*/
-#include "mbed.h"
-#include "HCSR04.h"
-
-HCSR04::HCSR04(PinName echoPin, PinName triggerPin) : echo(echoPin), trigger(triggerPin) {
-    init();
-}
-
-void HCSR04::init() {
-    /** configure the rising edge to start the timer */
-    echo.rise(this, &HCSR04::startTimer);
-    
-    /** configure the falling edge to stop the timer */
-    echo.fall(this, &HCSR04::stopTimer);
-    
-    distance = -1;      // initial distance
-    minDistance = 2;
-    maxDistance = 400;
-}
-
-void HCSR04::startTimer() {
-    timer.start(); // start the timer
-}
-
-void HCSR04::stopTimer() {
-    timer.stop(); // stop the timer
-}
-
-void HCSR04::startMeasurement() {
-    trigger = 1;
-    wait_us(10);
-    trigger = 0;
-    wait_ms(25); // just enough time to measure 400 cm
-    timer.stop(); // just in case echo fall did not occur
-    distance = timer.read() * 1e6 / 58;
-    if (distance < minDistance)
-        distance = minDistance;
-    if (distance > maxDistance)
-        distance = maxDistance;
-    timer.reset();
-}
-
-float HCSR04::getDistance_cm() {
-    startMeasurement();
-    return distance;
-}
-
-float HCSR04::getDistance_mm() {
-    startMeasurement();
-    return distance * 10;
-}
-
-void HCSR04::setRanges(float minRange, float maxRange) {
-    if (minRange < maxRange) {
-        if (minRange >= 2) 
-            minDistance = minRange;
-        if (maxRange <= 400)
-            maxDistance = maxRange;
-    }
-}
-
-float HCSR04::getMinRange() {
-    return minDistance;
-}
-
-float HCSR04::getMaxRange() {
-    return maxDistance;
-}
\ No newline at end of file

diff -r c2d4a02115db -r a852e63cac3d HCSR04.h
--- a/HCSR04.h	Thu Nov 24 09:15:48 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,87 +0,0 @@
-/*
-Dette er header filen for ultralydssensoren
-*/
-
-#ifndef HCSR04_H_TVZMT
-#define HCSR04_H_TVZMT
-
-/** A distance measurement class using ultrasonic sensor HC-SR04.
- *  
- * Example of use:
- * @code
- * #include "mbed.h"
- * #include "HCSR04.h"
- *
- * Serial pc(USBTX, USBRX);
- *
- * int main() {
- *     HCSR04 sensor(p5, p7);
- *     sensor.setRanges(10, 110);
- *     pc.printf("Min. range = %g cm\n\rMax. range = %g cm\n\r",
- *       sensor.getMinRange(), sensor.getMaxRange());
- *     while(1) {
- *         pc.printf("Distance: %5.1f mm\r", sensor.getDistance_mm());
- *         wait_ms(500);
- *     }
- * }
- * @endcode
- */
-class HCSR04 {
-    
-    public:
-    
-    /** Receives two PinName variables.
-     * @param echoPin mbed pin to which the echo signal is connected to
-     * @param triggerPin mbed pin to which the trigger signal is connected to
-     */
-    HCSR04(PinName echoPin, PinName triggerPin);
-    
-    /** Calculates the distance in cm, with the calculation time of approximatelly 23.7 ms.
-     * @returns distance of the measuring object in cm.
-     */
-    float getDistance_cm();
-    
-    /** Calculates the distance in mm, with the calculation time of approximatelly 23.7 ms.
-     * @returns distance of the measuring object in mm.
-     */
-    float getDistance_mm();
-    
-    /** Sets the minimum and maximum ranges between the factory values of 2 cm and 400 cm.
-     *  @param minRange Minimum range in cm. Must be between 2 cm and maxRange.
-     *  @param maxRange Maximum range in cm. Must be between minRange and 400 cm.
-     */
-    void setRanges(float minRange, float maxRange);
-    
-    /** Retreives the minimum sensor range set by the user.
-     * @returns the minimum sensor range set by the user in cm.
-     */
-    float getMinRange();
-    
-    /** Retreives the maximum sensor range set by the user.
-     * @returns the maximum sensor range set by the user in cm.
-     */
-    float getMaxRange();
-    
-    private:
-    
-    InterruptIn echo;       // echo pin
-    DigitalOut trigger;     // trigger pin
-    Timer timer;            // echo pulsewidth measurement
-    float distance;         // store the distance in cm
-    float minDistance;      // minimum measurable distance
-    float maxDistance;      // maximum measurable distance
-    
-    /** Start the timer. */
-    void startTimer();
-    
-    /** Stop the timer. */
-    void stopTimer();
-    
-    /** Initialization. */
-    void init();
-    
-    /** Start the measurement. */
-    void startMeasurement();
-};
-
-#endif
\ No newline at end of file

diff -r c2d4a02115db -r a852e63cac3d hack_motor.cpp
--- a/hack_motor.cpp	Thu Nov 24 09:15:48 2016 +0000
+++ b/hack_motor.cpp	Fri Dec 09 12:32:21 2016 +0000
@@ -47,8 +47,7 @@
 }
 
 void Wheel::FW(float right, float left) // set the actual speed for right, left motor
-{
-    
+{   
     M1A->write(right); //Set the duty cycle to the wanted percent, from speed variable
     M2A->write(left); // -//-
     *M1B = 0;

diff -r c2d4a02115db -r a852e63cac3d hack_motor.h
--- a/hack_motor.h	Thu Nov 24 09:15:48 2016 +0000
+++ b/hack_motor.h	Fri Dec 09 12:32:21 2016 +0000
@@ -1,11 +1,8 @@
+#include "mbed.h"
 #ifndef HACK_MOTOR_H
 #define HACK_MOTOR_H
-#include "mbed.h"
-
 class Wheel {
- 
 public:
-    
     Wheel(PinName M1A_pin, PinName M1B_pin, PinName M2A_pin, PinName M2B_pin);
     ~Wheel();
     void FW(); // Forward
@@ -17,7 +14,6 @@
     void set_speed(float speed); // set your wanted speed (0..1.0)
     float get_speed(); // get the actual speed
     void FW(float right, float left); // Overload Forward with right motor speed, left motor speed
-    
 private:
     float fw, bw;
     float speed;
@@ -25,6 +21,5 @@
     PwmOut *M2A;    
     DigitalOut *M1B; 
     DigitalOut *M2B;
-};
-
+}; 
 #endif
\ No newline at end of file

diff -r c2d4a02115db -r a852e63cac3d nucleo_servo.cpp
--- a/nucleo_servo.cpp	Thu Nov 24 09:15:48 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,30 +0,0 @@
-#include "mbed.h"
-#include "nucleo_servo.h"
-   
-Servo::Servo(PinName pin){
-    servoPWM = new PwmOut(pin);
-    servoPWM->period_ms(20);  //Do not change ;)
-    servoPWM->pulsewidth_us(600);
-    degrees = 0;
-}
-Servo::~Servo(){
-    delete servoPWM;  
-}
-
-void Servo::set_position(float deg){
-    degrees = deg;
-    if(degrees > 180)degrees = 180;
-    if(degrees < 0)degrees = 0;
-    int pw_us = 640 + (degrees / 180 * 1900); 
-    servoPWM->pulsewidth_us(pw_us);
-}
-    
-void Servo::turn_left(float deg){
-    degrees -= deg;
-    set_position(degrees);
-}
-
-void Servo::turn_right(float deg){
-    degrees += deg;
-    set_position(degrees);
-}

diff -r c2d4a02115db -r a852e63cac3d nucleo_servo.h
--- a/nucleo_servo.h	Thu Nov 24 09:15:48 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,18 +0,0 @@
-#ifndef _NUCLEO_H_
-#define _NUCLEO_H_
-
-class Servo {
-
-public:
-    Servo(PinName pin);
-    ~Servo();
-    void set_position(float deg);  
-    void turn_left(float deg); 
-    void turn_right(float deg); 
-private:
-    PwmOut *servoPWM;
-    float degrees;
-};
-    
-
-#endif
\ No newline at end of file

diff -r c2d4a02115db -r a852e63cac3d odometry.h
--- a/odometry.h	Thu Nov 24 09:15:48 2016 +0000
+++ b/odometry.h	Fri Dec 09 12:32:21 2016 +0000
@@ -1,154 +1,81 @@
 #ifndef ODOMETRY_H
 #define ODOMETRY_H
-
 #include "mbed.h"
 #include "hack_motor.h"
-#include "math.h"
+#include "math.h"   
 
 //  ODOMETRY VALUES
 #define N 20 // ticks on wheel
-#define R 32.5 // radius = 32.5 mm
+#define R 32.5 // radius = 32.5 mm on wheels
 #define L 133 // 133 mm distance between wheels
 #define PI 3.141592
+#define DISTANCE 3000
 int tickL = 0; // tick on left wheel
 int tickR = 0; // tick on right wheel
 
 //  PID VALUES
 #define P_TERM 1.0
-#define I_TERM 0.1
+#define I_TERM 0
 #define D_TERM 0
-#define MAX 1.0 
 #define MIN 0
+#define MAX 1.0
 
 //  GLOBAL DEFINITIONS
-double right,left;
-double speed=0.5;
+double right,left;  
+double result;
+double speed=0.5;  
+DigitalOut LED(LED1);
 
 //  ANALOG POWER
 AnalogIn   ain(PC_4);
 DigitalOut dout(PB_13);
 DigitalOut greenout(PB_12);
 int stop=0;         //DigitalOut DEFINITION OF RED LED!!
+DigitalOut LedFlash(PA_3);  //  RED LED WILL FLASH IF ROBOT STOPS 
 
-//  COORDINATES TO REACH
-const double xPosition [] =
-{
-    [0] = 0,
-    [1] = 1000,
-    [2] = 2000,
-    [3] = 3000,
-    [4] = 4000,
-    [5] = 5000,
-    [6] = 4000,
-    [7] = 3000,
-    [8] = 2000,
-    [9] = 1000,
-    [10] = 2000,
-    [11] = 3000,
-    [12] = 4000,
-    [13] = 5000,
-    [14] = 1000
-};
-const double yPosition [] = 
-{
-    [0] = 0,
-    [1] = 2000,
-    [2] = 3000,
-    [3] = 4000,
-    [4] = 5000,
-    [5] = 2000,
-    [6] = 4000,
-    [7] = 3000,
-    [8] = 2000,
-    [9] = 1000,
-    [10] = 3000,
-    [11] = 2000,
-    [12] = 1000,
-    [13] = 2000,
-    [14] = 3000
-};
-const double THETA [] =
-{
-    [0] = 0.785,
-    [1] = 1.047,
-    [2] = 0.698,
-    [3] = 0.524,
-    [4] = 1.047,
-    [5] = 0.698,
-    [6] = 0.785,
-    [7] = 0.873,
-    [8] = 0.349,
-    [9] = 0.349,
-    [10] = 0.611,
-    [11] = 0.611,
-    [12] = 0.436,
-    [13] = 0.96
-}; //INCLUDE ANGLE TO HEAD BACK TO ORIGIN POSITION!!!!!
-
-const double DISTANCES [] = 
-{
-    [0] = sqrt(pow((xPosition[1] - xPosition[0]), 2) + pow((yPosition[1] - yPosition[0]), 2)),
-    [1] = sqrt(pow((xPosition[2] - xPosition[1]), 2) + pow((yPosition[2] - yPosition[1]), 2)),
-    [2] = sqrt(pow((xPosition[3] - xPosition[2]), 2) + pow((yPosition[3] - yPosition[2]), 2)),
-    [3] = sqrt(pow((xPosition[4] - xPosition[3]), 2) + pow((yPosition[4] - yPosition[3]), 2)),
-    [4] = sqrt(pow((xPosition[5] - xPosition[4]), 2) + pow((yPosition[5] - yPosition[4]), 2)),
-    [5] = sqrt(pow((xPosition[6] - xPosition[5]), 2) + pow((yPosition[6] - yPosition[5]), 2)),
-    [6] = sqrt(pow((xPosition[7] - xPosition[6]), 2) + pow((yPosition[7] - yPosition[6]), 2)),
-    [7] = sqrt(pow((xPosition[8] - xPosition[7]), 2) + pow((yPosition[8] - yPosition[7]), 2)),
-    [8] = sqrt(pow((xPosition[9] - xPosition[8]), 2) + pow((yPosition[9] - yPosition[8]), 2)),
-    [9] = sqrt(pow((xPosition[10] - xPosition[9]), 2) + pow((yPosition[10] - yPosition[9]), 2)),
-    [10] = sqrt(pow((xPosition[11] - xPosition[10]), 2) + pow((yPosition[11] - yPosition[10]), 2)),
-    [11] = sqrt(pow((xPosition[12] - xPosition[11]), 2) + pow((yPosition[12] - yPosition[11]), 2)),
-    [12] = sqrt(pow((xPosition[13] - xPosition[12]), 2) + pow((yPosition[13] - yPosition[12]), 2)),
-    [13] = sqrt(pow((xPosition[14] - xPosition[13]), 2) + pow((yPosition[14] - yPosition[13]), 2))
-};  //REMEMBER TO INCLUDE DISTANCE FOR TRAVELLING BACK AGAIN!
-
-/*
-=============================================================================
-TIMER, TACHO'S AND ATTACHED PINS TO H-BRIDGE
-=============================================================================*/
+/////////////////////////////////////////////////////////////////////////////
+//              TIMER, TACHO'S AND ATTACHED PINS TO H-BRIDGE               //
+/////////////////////////////////////////////////////////////////////////////
 Timer t; // DEFINE A TIMER T
-Serial pc(USBTX, USBRX); // not used here because we only have one serial
+Serial pc(USBTX, USBRX); // not used here because we only have one serial 
 // connection
 InterruptIn tacho_left(PC_3); // Set PC_2 to be interupt in for tacho left - Ledningsfarve: Hvid
-InterruptIn tacho_right(PC_2);// Set PC_3 to be interupt in for tacho right - Ledningsfarve: Grå
-
+InterruptIn tacho_right(PC_2);// Set PC_3 to be interupt in for tacho right - Ledningsfarve: Grå    
 Wheel robot(PB_2, PB_1, PB_15, PB_14);  //PB_15: grøn PB_14: blå PB_2: orange PB_1: gul
 // create an object of robot H-bridge. ---(M1A, M1B, M2A, M2B)---
 
-/*      USED FUNCTIONS IN ROBOT.CPP*/
-
+    //  GETTING INFO FROM SENSORS 
 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;
+        stop=0; 
     } else {
         dout = not dout;
         stop=1; // flash red led
+        LED = 1;
     }
-
     if (ain==1.0f) greenout = 1; // full power
     else greenout = 0;
 }
-
 //  INIT YOUR PARAMETERS
 void init()
 {
     tickL=0;
     tickR=0;
 }
-
 //  SENSOR INFO FROM TACO SENSORS
 void tickLeft() // UPDATE LEFT TICK ON INTERRUPT
 {
     tickL++;
 }
+
 void tickRight() // UPDATE RIGHT TICK ON INTERRUPT
 {
     tickR++;
 }
+    
 float Dleft() // DISTANCE FOR LEFT WHEEL
 {
     return 2*PI*R*tickL/N;
@@ -163,93 +90,72 @@
 {
     return (Dleft()+Dright())/2;
 }
-
-float DeltaTHETA()  //UPDATE ON ANGLE
+void turn( )
 {
-    return (Dright()-Dleft())/2*L;
-}
-
-    //  X POSITIONS
-float DeltaX_1()    //  1. X POSITION
-{
-    return Dcenter()*cos(THETA[0]+DeltaTHETA()/2);
+    
 }
 
-    //  Y POSITIONS
-float DeltaY_1()    //1. Y POSITION
-{
-    return Dcenter()*sin(THETA[0]+DeltaTHETA()/2);
-}
-    //  DISTANCES TRAVELLED 
-float Dtravel_1()   //  1. DISTANCE
-{
-    return sqrt(pow(DeltaX_1(), 2) + pow(DeltaY_1(), 2));
-}
-
-    //  PID REGULATOR
-void get_to_goal ( ) // FUNCTION TO REACH GOAL WITH ERROR CALCULATION   
+/////////////////////////////////////////////////////////////////////
+//                      PID REGULATOR                              //
+/////////////////////////////////////////////////////////////////////
+void get_to_goal ( )    
 {   
-    double e = 0; // angle error
-    double THETA_D1 = atan(DeltaY_1() / DeltaX_1());
-    
+    double e = 0; 
     double output = 0;
     double derivative = 0;
     double proportional = 0;
     double integral = 0;
-    double e_old = 0;   
+    double e_old = 0;
 
-    while (Dtravel_1() <= DISTANCES[0])    //  INDSTIL TIL RIGTIGE PUNKTER!!! SKER I ET PARALLELT TIDSFORLØB MED KØRSEL AF ROBOT TIL PUNKTER
-    {
-        if(THETA_D1 < -PI)    //  EVALUATES IF THE ANGLE IS LESS THAN -3.14
+    while (Dcenter() <= DISTANCE) 
+    {    
+        e = tickR - tickL;  //error calculation
+        if(e < 0)
         {
-            THETA_D1 = -PI;
+            e = 0;   
+        }
+        else if(e > 0)
+        {
+         e = 0;   
         }
     
-        else if(THETA_D1 > PI)  //  EVALUATES IF THE ANGLE IS MORE THAN 3.14
-        {
-            THETA_D1 = PI;   
-        }
-        
-        e = THETA_D1 - THETA[0];    //  ERROR VALUE
-
-        // Compute the proportional
-        proportional = e; // get the error
-
-        // Compute the integral
-        integral += proportional;
-
-        // Compute the derivative
+        //PID calculation
+        proportional = e; // GETTING THE ERROR VALUE
+        integral += proportional;   // THE ERROR NEVER GETS TOO LOW
         derivative = e - e_old;
-
-        // Remember the last error.
         e_old = e;
-
-        // Compute the output
-        output = (proportional * (P_TERM)) + (integral * (I_TERM))
-                + (derivative * (D_TERM));
-
-        // Compute new speeds of both wheels
-        right = speed - output;//if power is 0, no error and same speed on wheels
+        
+        // COMPUTING THE OUTPUT SIGNAL
+        output = (proportional * (P_TERM)) + (integral * (I_TERM))+ 
+        (derivative * (D_TERM));
+        
+        // COMPUTING NEW SPEEDS ON WHEELS
+        right = speed - output;//if power is 0, no error->same speed on wheels
         left = speed + output;
-
-        // limit checks
-        if (right < MIN)
+        
+        //CHECK YOUR LIMITS
+        if(right < MIN)
+        {
             right = MIN;
-        else if (right > MAX)
+        }
+        else if(right > MAX)
+        {
             right = MAX;
-
-        if (left < MIN)
+        }
+        if(left < MIN)
+        {
             left = MIN;
-        else if (left > MAX)
+        }
+        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);
-               
-        printf("\n\r Error: %.2lf", e); //  HUSK DETTE!
-
-        wait_ms(5000); // should be adjusted to your context. Give the motor time
+        }
+        printf("\n\r RightSpeed: %.2lf  RightTicks: %d  LeftSpeed: %.2lf"  
+        "LeftTicks: %d", right, tickR, left, tickL);    
+    
+        // set new positions
+        robot.FW(right,left); 
+        wait_ms(25); // should be adjusted to your context. Give the motor time
         // to do something before you react
     }
     t.stop(); // stop timer

diff -r c2d4a02115db -r a852e63cac3d robot.cpp
--- a/robot.cpp	Thu Nov 24 09:15:48 2016 +0000
+++ b/robot.cpp	Fri Dec 09 12:32:21 2016 +0000
@@ -7,76 +7,64 @@
  Copyright   : Open for all
  Description : Program to serve the platform for Pro1 2016
  ============================================================================
- */
-#include "odometry.h"
+ */ 
+#include "odometry.h" 
 #include "mbed.h"
-#include "HCSR04.h"
-#include "nucleo_servo.h"
 #include "hack_motor.h"
 #include <math.h> 
-#include "PID.h"
-
 
 void startfunction( );
-Ticker T1; // create an object T1 of Ticker
+Ticker T1; // create an object T1 of Ticker 
 
-/*
-----------------------------------------------------------------------------
-                                MAIN FUNCTION
-----------------------------------------------------------------------------*/
 int main()
-{   
-/*
-=============================================================================
-Driving and stopping the robot with member functions from the wheel class
-=============================================================================*/
-    startfunction();    //  1. DRIVE     
+{ 
+/////////////////////////////////////////////////////////////////////////////
+//                          Driving setup                                  // 
+/////////////////////////////////////////////////////////////////////////////
+    while(1)    //Remember to include if statement to stop robot if it's time to recharge
+    {
+    read_analog();
+    startfunction(); 
+    get_to_goal();
+    robot.stop();
+    printf("\n\r DistanceBysensor: %.2f \n", Dcenter()); 
+    wait_ms(2000);
+    
+    read_analog();
+    startfunction();
     get_to_goal();
-    robot.stop(); 
-    printf("\n\rtimeused = %4d tickL= %4d  tickR = %4d \n", t.read_ms(),tickL,
-           tickR);   
-    wait_ms(3000);
+    robot.stop();
+    printf("\n\r DistanceBysensor: %.2f \n", Dcenter());  
+    wait_ms(2000);
+    
+    read_analog();
+    startfunction();
+    get_to_goal();
+    robot.stop();
+    printf("\n\r DistanceBysensor: %.2f \n", Dcenter()); 
+    wait_ms(2000);
+    
+    read_analog();
+    startfunction();
+    get_to_goal();
+    robot.stop();
+    printf("\n\r DistanceBysensor: %.2f \n", Dcenter());  
+    wait_ms(2000);
+    }
 } 
 /*  
 ----------------------------------------------------------------------------
                             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
-
     tacho_left.rise(&tickLeft);  // attach the address of the count function
     //to the falling edge
     tacho_right.rise(&tickRight);  // attach the address of the count function
     //to the falling edge
-
-    HCSR04 sensor(PC_5,PC_6); // Create an object of HCSR04 ultrasonic with pin
-    // (echo,trigger) on pin PC_5, PC6
-    Servo servo1(PC_8);     //Create an object of Servo class on pin PC_8
-
-    sensor.setRanges(2, 400); // set the range for Ultrasonic
-
-/*
-=============================================================================
-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 \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 \n", sensor.getDistance_mm());
-    servo1.set_position(90); // Straight ahead (angle = 90 degree) for servo
-    wait_ms (500);
-    printf("\r\nDistance: %5.1f mm \n", sensor.getDistance_mm());
-
     init(); // initialise parameters (just for you to remember if you need to)
-
-    wait_ms(1000); //wait 1 secs here before you go
+    wait_ms(2000); //wait 1 secs here before you go
     t.start();      // start timer (just demo of how you can use a timer)
 }
\ No newline at end of file