Diff: robot.cpp

Revision:

0:d3dbe632b1a9

Child:

1:396a582e8861

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/robot.cpp	Mon Oct 31 09:57:07 2016 +0000
@@ -0,0 +1,266 @@
+/*
+ ============================================================================
+ Name        : robot.cpp
+ Author      : Henning Slavensky
+ Version     : 0.1
+ Date        : 13102016
+ Copyright   : Open for all
+ Description : Program to serve af platform for Pro1 2016
+ ============================================================================
+ */
+#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
+
+// PID definitions
+#define P_TERM 0.1
+#define I_TERM 0
+#define D_TERM 0.0
+#define MAX 1.0
+#define MIN 0
+
+
+/* ------------------------------
+    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
+   ------------------------------
+*/
+                
+#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 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
+
+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
+
+/*  -----------
+    Prototypes
+    -----------
+*/
+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
+void get_to_goal ( ); // function to get to goal
+void read_analog(); // comes here every second
+
+Ticker T1; // create an object T1 of Ticker
+
+int main()
+{
+
+    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", 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());
+    servo1.set_position(90); // Straight ahead (angle = 90 degree) for servo
+    wait_ms (500);
+    printf("\r\nDistance: %5.1f mm", 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
+    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
+    // ------------------------------------------------------------
+    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);
+    }
+    // --------------------------------------------------------------
+
+    robot.stop(); // stop the robot again
+    printf("\n\rtimeused = %4d tickL= %4d  tickR = %4d ", t.read_ms(),tickL,
+           tickR);
+
+}
+
+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;
+}
+
+
+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) {
+
+        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
+         * --------------------------
+         */
+
+        // 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 adusted 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