robot.cpp

00001 /*
00002  ============================================================================
00003  Name        : robot.cpp
00004  Author      : Henning Slavensky
00005  Version     : 0.1
00006  Date        : 13102016
00007  Copyright   : Open for all
00008  Description : Program to serve af platform for Pro1 2016
00009  ============================================================================
00010  */
00011 #include "mbed.h"
00012 #include "HCSR04.h"
00013 #include "nucleo_servo.h"
00014 #include "hack_motor.h"
00015 #include <math.h> // nice when you got to work with real geometry stuff
00016 
00017 #define PI 3.141592 // definition of PI
00018 
00019 // PID definitions
00020 #define P_TERM 0.1
00021 #define I_TERM 0
00022 #define D_TERM 0.0
00023 #define MAX 1.0
00024 #define MIN 0
00025 
00026 
00027 /* ------------------------------
00028     Definitions for robot
00029     note that N,R,L maybe not have the most meaningful name
00030     but they follow the names from the theory for our
00031     robot lectures, and can be used in youa application
00032    ------------------------------
00033 */
00034                 
00035 #define N 20 // ticks on wheel
00036 #define R 32.5 // radius = 32.5 mm
00037 #define L 133 // 133 mm distance between wheels
00038 
00039 //Coordinates for Robot with x and y
00040 #define starting_x = 0
00041 #define starting_y = 0 
00042 #define second_x = 5
00043 #define second_y = 4
00044 #define third_x = 7
00045 #define third_y = 1
00046 
00047 double distcalc(double, double)
00048 {
00049     
00050 }
00051 
00052 Timer t; // define a timer t
00053 Serial pc(USBTX, USBRX); // not used here because we only have one serial
00054 // connection
00055 InterruptIn tacho_left(PC_3); // Set PC_3 to be interupt in for tacho left
00056 InterruptIn tacho_right(PC_2);// Set PC_2 to be interupt in for tacho right
00057 
00058 Wheel robot(PB_2, PB_1, PB_15, PB_14);  // create an object of robot H-bridge
00059 // (M1A, M1B, M2A, M2B)
00060 
00061 /*  --------------------------- 
00062     definition for analog power
00063     ---------------------------
00064 */
00065 AnalogIn   ain(PC_4);
00066 DigitalOut dout(PB_13);
00067 DigitalOut greenout(PB_12);
00068 
00069 int stop=0;
00070 
00071 /*  ---------------------
00072         Global variables
00073     --------------------
00074     */
00075 double right,left;
00076 double speed=0.5;
00077 double e = 0; // angle error
00078 int tickL = 0; // tick on left wheel
00079 int tickR = 0; // tick on right wheel
00080 
00081 /*  -----------
00082     Prototypes
00083     -----------
00084 */
00085 void init();
00086 void tickLeft(); // read tick left
00087 void tickRight(); // read tick right
00088 float Dleft(); // distance left wheel
00089 float Dright(); // distance right wheel
00090 float Dcenter(); // Distance for center
00091 void get_to_goal ( ); // function to get to goal
00092 void read_analog(); // comes here every second
00093 
00094 Ticker T1; // create an object T1 of Ticker
00095 
00096 int main()
00097 {
00098 
00099     T1.attach(&read_analog, 1.0); // attach the address of the read_analog
00100     //function to read analog in every second
00101 
00102     tacho_left.rise(&tickLeft);  // attach the address of the count function
00103     //to the falling edge
00104     tacho_right.rise(&tickRight);  // attach the address of the count function
00105     //to the falling edge
00106 
00107     HCSR04 sensor(PC_5,PC_6); // Create an object of HCSR04 ultrasonic with pin
00108     // (echo,trigger) on pin PC_5, PC6
00109     Servo servo1(PC_8);     //Create an object of Servo class on pin PC_8
00110 
00111     sensor.setRanges(2, 400); // set the range for Ultrasonic
00112 
00113     /*  -----------------------------------------
00114         Demo of the servor and ulta sonic sensor
00115         -----------------------------------------
00116     */
00117     wait_ms(2000); // just get time for you to enable your screeen
00118     servo1.set_position(0); // Servo right position (angle = 0 degree) for servo
00119     wait_ms (500);
00120     printf("\r\nDistance: %5.1f mm", sensor.getDistance_mm()); // display the
00121     //readings from ultra sonic at this position
00122     servo1.set_position(180); // Servo left position (angle = 180 degree)
00123     //for servo
00124     wait_ms (500);
00125     printf("\r\nDistance: %5.1f mm", sensor.getDistance_mm());
00126     servo1.set_position(90); // Straight ahead (angle = 90 degree) for servo
00127     wait_ms (500);
00128     printf("\r\nDistance: %5.1f mm", sensor.getDistance_mm());
00129 
00130     init(); // initialise parameters (just for you to remember if you need to)
00131 
00132     wait_ms(1000); //wait 1 secs here before you go
00133     t.start();      // start timer (just demo of how you can use a timer)
00134 
00135     /*  ------------------
00136             with PID
00137         -----------------
00138     --  delete get_to_goal(); when you use without pid
00139     get_to_goal ();
00140     */
00141 
00142     /* ---------------------------
00143            without pid
00144        ------------------------
00145     */
00146     // delete between --- when you use it with pid
00147     // ------------------------------------------------------------
00148     while (Dcenter() <= CLOSE_ENOUGH) { // while distance traveled is less than
00149         //target
00150         
00151             robot.FW(0.5,0.5); // set new values half speed on both wheels
00152             wait_ms(20);
00153     }
00154     // --------------------------------------------------------------
00155 
00156     robot.stop(); // stop the robot again
00157     printf("\n\rtimeused = %4d tickL= %4d  tickR = %4d ", t.read_ms(),tickL,
00158            tickR);
00159 
00160 }
00161 
00162 void tickLeft() // udtate left tick on tacho interrupt
00163 {
00164     tickL++;
00165 }
00166 void tickRight() // udtate right tick on tacho interrupt
00167 {
00168     tickR++;
00169 }
00170 float Dleft() // Distance for left wheel
00171 {
00172 
00173     return 2*PI*R*tickL/N;
00174 }
00175 
00176 float Dright() // Distance for right wheel
00177 {
00178 
00179     return 2*PI*R*tickR/N;
00180 }
00181 
00182 float Dcenter() // Distance for center
00183 {
00184     return (Dleft()+Dright())/2;
00185 }
00186 
00187 
00188 void get_to_goal ( ) // function to get to goal
00189 {
00190 
00191     // PID init
00192     double power, derivative, proportional, integral, e_old;
00193     power=derivative=proportional=integral=e_old=0;
00194 
00195     while (Dcenter() < CLOSE_ENOUGH) {
00196 
00197         e = tickR-tickL; // error is difference between right tick and left tick
00198         // should be ajusted to your context, angle for robot
00199         // and instead of test Dcenter() in your while loop, test
00200         // for distance_to_goal()
00201 
00202         /* --------------------------
00203          * PID regulator
00204          * --------------------------
00205          */
00206 
00207         // Compute the proportional
00208         proportional = e; // get the error
00209 
00210         // Compute the integral
00211         integral += proportional;
00212 
00213         // Compute the derivative
00214         derivative = e - e_old;
00215 
00216         // Remember the last error.
00217         e_old = e;
00218 
00219         // Compute the power
00220         power = (proportional * (P_TERM)) + (integral * (I_TERM))
00221                 + (derivative * (D_TERM));
00222 
00223         // Compute new speeds
00224 
00225         right = speed - power;//if power is 0, no error and same speed on wheels
00226         left = speed + power;
00227 
00228 
00229         // limit checks
00230         if (right < MIN)
00231             right = MIN;
00232         else if (right > MAX)
00233             right = MAX;
00234 
00235         if (left < MIN)
00236             left = MIN;
00237         else if (left > MAX)
00238             left = MAX;
00239 
00240         robot.FW(right,left); // set new positions
00241         printf("\n\r Dcenter = %3.2f tickL= %4d  tickR = %4d left %3.2f right %3.2f",
00242                Dcenter(),tickL,tickR,left,right);
00243 
00244         wait_ms(20); // should be adusted to your context. Give the motor time
00245         // to do something before you react
00246 
00247     }
00248 
00249     t.stop(); // stop timer
00250 
00251 }
00252 
00253 void read_analog() // comes here every second in this case
00254 // could be flagged with global variables like "stop"
00255 {
00256     if(ain > 0.6f) { // 60% power, time for recharge
00257         dout = 1;
00258         stop=0;
00259     } else {
00260         dout = not dout;
00261         stop=1; // flash red led
00262     }
00263 
00264     if (ain==1.0f) greenout = 1; // full power
00265     else greenout = 0;
00266 }
00267 void init() // init your parameters
00268 {
00269     tickL=0;
00270     tickR=0;
00271 }