Kildekode

Dependencies:   mbed

Fork of Team5_kode by Kim Nielsen

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers robot.cpp Source File

robot.cpp

00001 /*
00002  ============================================================================
00003  Name        : robot.cpp
00004  Author      : Team 5
00005  Version     : 0.1
00006  Date        : 13-10-2016
00007  Copyright   : Open for all
00008  Description : Program to serve af platform for Pro1 2016
00009  ============================================================================
00010  */
00011     
00012 /*
00013 =============================================================================
00014 Including the necessary header files
00015 =============================================================================
00016 */
00017 #include "mbed.h"
00018 #include "HCSR04.h"
00019 #include "nucleo_servo.h"
00020 #include "hack_motor.h"
00021 #include <math.h> 
00022 
00023 /*
00024 =============================================================================
00025 PID definitions
00026 =============================================================================
00027 */
00028 #define P_TERM 0.2
00029 #define I_TERM 0.1
00030 #define D_TERM 0.3
00031 #define MAX 1.0 
00032 #define MIN 0
00033 #define PI 3.141592
00034 
00035 /*
00036 =============================================================================
00037 Geometry for robot:
00038 note that N,R,L maybe not have the most meaningful name
00039 but they follow the names from the theory for our
00040 robot lectures, and can be used in youa application
00041 =============================================================================
00042 */         
00043 #define N 20 // ticks on wheel
00044 #define R 32.5 // radius = 32.5 mm
00045 #define L 133 // 133 mm distance between wheels
00046  
00047 /*
00048 =============================================================================
00049 Coordinates for robot to reach
00050 =============================================================================
00051 */
00052 //Start position:
00053 #define first_x 0
00054 #define first_y 0
00055 #define first_angle 0.785  //45 deg
00056 
00057 //Second position:
00058 #define second_x 50
00059 #define second_y 40
00060 #define second_angle 1.047 //60 deg
00061 
00062 //Third position:
00063 #define third_x 70
00064 #define third_y 10
00065 #define third_angle 1.396 //80 deg
00066 
00067 //fourth position:
00068 #define fourth_x 70
00069 #define fourth_y 50
00070   
00071 Timer t; // define a timer t
00072 Serial pc(USBTX, USBRX); // not used here because we only have one serial
00073 // connection
00074 InterruptIn tacho_left(PC_3); // Set PC_3 to be interupt in for tacho left
00075 InterruptIn tacho_right(PC_2);// Set PC_2 to be interupt in for tacho right
00076 
00077 Wheel robot(PB_2, PB_1, PB_15, PB_14);  
00078 // create an object of robot H-bridge
00079 // (M1A, M1B, M2A, M2B)
00080 
00081 /*
00082 =============================================================================
00083 definition for analog power
00084 =============================================================================
00085 */
00086 AnalogIn   ain(PC_4);
00087 DigitalOut dout(PB_13);
00088 DigitalOut greenout(PB_12);
00089 
00090 int stop=0;
00091 /*
00092 =============================================================================
00093 Global variables
00094 =============================================================================
00095 */    
00096 double right,left;
00097 double speed=0.5;
00098 double e = 0; // angle error
00099 int tickL = 0; // tick on left wheel
00100 int tickR = 0; // tick on right wheel
00101 
00102 /*
00103 =============================================================================
00104 Prototype defined functions
00105 =============================================================================
00106 */ 
00107 void init();
00108 void tickLeft(); // read tick left
00109 void tickRight(); // read tick right
00110 float Dleft(); // distance left wheel
00111 float Dright(); // distance right wheel
00112 float Dcenter(); // Distance for center
00113 
00114 //functions for calculating desired distances from defined points
00115 float Dist_reach1();
00116 float Dist_reach2();
00117 float Dist_reach3();
00118 
00119 void get_to_goal ( ); // function to get to goal
00120 void read_analog(); // comes here every second
00121 
00122 Ticker T1; // create an object T1 of Ticker
00123 
00124 /*
00125 ----------------------------------------------------------------------------
00126                             Accessing the int main
00127 ----------------------------------------------------------------------------
00128 */
00129 int main()
00130 {
00131     T1.attach(&read_analog, 1.0); // attach the address of the read_analog
00132     //function to read analog in every second
00133 
00134     tacho_left.rise(&tickLeft);  // attach the address of the count function
00135     //to the falling edge
00136     tacho_right.rise(&tickRight);  // attach the address of the count function
00137     //to the falling edge
00138 
00139     HCSR04 sensor(PC_5,PC_6); // Create an object of HCSR04 ultrasonic with pin
00140     // (echo,trigger) on pin PC_5, PC6
00141     Servo servo1(PC_8);     //Create an object of Servo class on pin PC_8
00142 
00143     sensor.setRanges(2, 400); // set the range for Ultrasonic
00144 
00145 /*
00146 =============================================================================
00147 Demo of the servor and ulta sonic sensor
00148 =============================================================================
00149 */  
00150     wait_ms(2000); // just get time for you to enable your screeen
00151     servo1.set_position(0); // Servo right position (angle = 0 degree) for servo
00152     wait_ms (500);
00153     printf("\r\nDistance: %5.1f mm \n", sensor.getDistance_mm()); // display the
00154     //readings from ultra sonic at this position
00155     servo1.set_position(180); // Servo left position (angle = 180 degree)
00156     //for servo
00157     wait_ms (500);
00158     printf("\r\nDistance: %5.1f mm \n", sensor.getDistance_mm());
00159     servo1.set_position(90); // Straight ahead (angle = 90 degree) for servo
00160     wait_ms (500);
00161     printf("\r\nDistance: %5.1f mm \n", sensor.getDistance_mm());
00162 
00163     init(); // initialise parameters (just for you to remember if you need to)
00164 
00165     wait_ms(1000); //wait 1 secs here before you go
00166     t.start();      // start timer (just demo of how you can use a timer)
00167 
00168     /*  ------------------
00169             with PID
00170         -----------------
00171     --  delete get_to_goal(); when you use without pid
00172     get_to_goal ();
00173     */
00174 
00175     /* ---------------------------
00176            without pid
00177        ------------------------
00178     */
00179     // delete between --- when you use it with pid
00180     // ------------------------------------------------------------
00181 /*
00182 =============================================================================
00183 Driving and stopping the robot with member functions from the wheel class
00184 =============================================================================
00185 */
00186     while(Dcenter() <= Dist_reach1())
00187     {
00188         robot.FW(0.5, 0.5);
00189         wait_ms(5000);
00190     }
00191     robot.stop(); // stop the robot again
00192     printf("\n\rtimeused = %4d tickL= %4d  tickR = %4d ", t.read_ms(),tickL,
00193            tickR);    
00194            
00195 } 
00196 /*  
00197 ----------------------------------------------------------------------------
00198                             END OF MAIN FUNCTION
00199 ----------------------------------------------------------------------------
00200 */
00201 
00202 /*
00203 =============================================================================
00204 Calculations of distances traveled by both wheels and robot
00205 =============================================================================
00206 */
00207 void tickLeft() // udtate left tick on tacho interrupt
00208 {
00209     tickL++;
00210 }
00211 void tickRight() // udtate right tick on tacho interrupt
00212 {
00213     tickR++;
00214 }
00215 float Dleft() // Distance for left wheel
00216 {
00217     return 2*PI*R*tickL/N;
00218 }
00219 
00220 float Dright() // Distance for right wheel
00221 {
00222     return 2*PI*R*tickR/N;
00223 }
00224 
00225 float Dcenter() // Distance for center
00226 {
00227     return (Dleft()+Dright())/2;
00228 }
00229 
00230 //Calculation of current position and new position with angles!!!
00231 
00232 float angle_new()
00233 {
00234     return (Dright()-Dleft())/L;   
00235 }
00236 
00237 float x_position()
00238 {
00239     return Dcenter()*sin(angle_new());   
00240 }
00241 
00242 float y_position()
00243 {
00244     return Dcenter()*cos(angle_new());
00245 }
00246 
00247 //slet efterfølgende
00248 float Dist_reach1() 
00249 { 
00250   double firstTosecond_calc = ((second_x-first_x)^2)+((second_y-first_y)^2);
00251   double dist_firstTosecond = sqrt(firstTosecond_calc); 
00252   
00253   return (dist_firstTosecond);
00254 }
00255 
00256 /*
00257 =============================================================================
00258 PID init
00259 =============================================================================
00260 */
00261 void get_to_goal ( ) // function to get to goal
00262 {
00263     double power = 0;
00264     double derivative = 0;
00265     double proportional = 0;
00266     double integral = 0;
00267     double e_old = 0;
00268 
00269     while (Dcenter() < Dist_reach1()) {
00270 /*
00271 -------------------------------------------------
00272 error is difference between right tick and left tick
00273 should be ajusted to your context, angle for robot
00274 and instead of test Dcenter() in your while loop, test
00275 for distance_to_goal()
00276 -------------------------------------------------
00277 */
00278         e = tickR-tickL; 
00279 /*
00280 =============================================================================
00281 PID regulator
00282 =============================================================================
00283 */
00284 
00285         // Compute the proportional
00286         proportional = e; // get the error
00287 
00288         // Compute the integral
00289         integral += proportional;
00290 
00291         // Compute the derivative
00292         derivative = e - e_old;
00293 
00294         // Remember the last error.
00295         e_old = e;
00296 
00297         // Compute the power
00298         power = (proportional * (P_TERM)) + (integral * (I_TERM))
00299                 + (derivative * (D_TERM));
00300 
00301         // Compute new speeds
00302 
00303         right = speed - power;//if power is 0, no error and same speed on wheels
00304         left = speed + power;
00305 
00306 
00307         // limit checks
00308         if (right < MIN)
00309             right = MIN;
00310         else if (right > MAX)
00311             right = MAX;
00312 
00313         if (left < MIN)
00314             left = MIN;
00315         else if (left > MAX)
00316             left = MAX;
00317 
00318         robot.FW(right,left); // set new positions
00319         printf("\n\r Dcenter = %3.2f tickL= %4d  tickR = %4d left %3.2f right %3.2f",
00320                Dcenter(),tickL,tickR,left,right);
00321 
00322         wait_ms(20); // should be adjusted to your context. Give the motor time
00323         // to do something before you react
00324     }
00325     t.stop(); // stop timer
00326 }
00327 
00328 void read_analog() // comes here every second in this case
00329 // could be flagged with global variables like "stop"
00330 {
00331     if(ain > 0.6f) { // 60% power, time for recharge
00332         dout = 1;
00333         stop=0;
00334     } else {
00335         dout = not dout;
00336         stop=1; // flash red led
00337     }
00338 
00339     if (ain==1.0f) greenout = 1; // full power
00340     else greenout = 0;
00341 }
00342 void init() // init your parameters
00343 {
00344     tickL=0;
00345     tickR=0;
00346 }