Kildekode

Dependencies:   mbed

Fork of PRO1 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 =============================================================================
00072 Calculation of length between points
00073 =============================================================================
00074 */
00075   double angle_desire = (second_y-first_y)/(second_x-first_x); 
00076   double error_present = atan(angle_desire)-0.785; 
00077   
00078 Timer t; // define a timer t
00079 Serial pc(USBTX, USBRX); // not used here because we only have one serial
00080 // connection
00081 InterruptIn tacho_left(PC_3); // Set PC_3 to be interupt in for tacho left
00082 InterruptIn tacho_right(PC_2);// Set PC_2 to be interupt in for tacho right
00083 
00084 Wheel robot(PB_2, PB_1, PB_15, PB_14);  
00085 // create an object of robot H-bridge
00086 // (M1A, M1B, M2A, M2B)
00087 
00088 /*
00089 =============================================================================
00090 definition for analog power
00091 =============================================================================
00092 */
00093 AnalogIn   ain(PC_4);
00094 DigitalOut dout(PB_13);
00095 DigitalOut greenout(PB_12);
00096 
00097 int stop=0;
00098 /*
00099 =============================================================================
00100 Global variables
00101 =============================================================================
00102 */    
00103 double right,left;
00104 double speed=0.5;
00105 double e = 0; // angle error
00106 int tickL = 0; // tick on left wheel
00107 int tickR = 0; // tick on right wheel
00108 
00109 /*
00110 =============================================================================
00111 Prototype defined functions
00112 =============================================================================
00113 */ 
00114 void init();
00115 void tickLeft(); // read tick left
00116 void tickRight(); // read tick right
00117 float Dleft(); // distance left wheel
00118 float Dright(); // distance right wheel
00119 float Dcenter(); // Distance for center
00120 
00121 //functions for calculating desired distances from defined points
00122 float Dist_reach1();
00123 float Dist_reach2();
00124 float Dist_reach3();
00125 
00126 void get_to_goal ( ); // function to get to goal
00127 void read_analog(); // comes here every second
00128 
00129 Ticker T1; // create an object T1 of Ticker
00130 
00131 /*
00132 ----------------------------------------------------------------------------
00133                             Accessing the int main
00134 ----------------------------------------------------------------------------
00135 */
00136 int main()
00137 {
00138     T1.attach(&read_analog, 1.0); // attach the address of the read_analog
00139     //function to read analog in every second
00140 
00141     tacho_left.rise(&tickLeft);  // attach the address of the count function
00142     //to the falling edge
00143     tacho_right.rise(&tickRight);  // attach the address of the count function
00144     //to the falling edge
00145 
00146     HCSR04 sensor(PC_5,PC_6); // Create an object of HCSR04 ultrasonic with pin
00147     // (echo,trigger) on pin PC_5, PC6
00148     Servo servo1(PC_8);     //Create an object of Servo class on pin PC_8
00149 
00150     sensor.setRanges(2, 400); // set the range for Ultrasonic
00151 
00152 /*
00153 =============================================================================
00154 Demo of the servor and ulta sonic sensor
00155 =============================================================================
00156 */  
00157     wait_ms(2000); // just get time for you to enable your screeen
00158     servo1.set_position(0); // Servo right position (angle = 0 degree) for servo
00159     wait_ms (500);
00160     printf("\r\nDistance: %5.1f mm \n", sensor.getDistance_mm()); // display the
00161     //readings from ultra sonic at this position
00162     servo1.set_position(180); // Servo left position (angle = 180 degree)
00163     //for servo
00164     wait_ms (500);
00165     printf("\r\nDistance: %5.1f mm \n", sensor.getDistance_mm());
00166     servo1.set_position(90); // Straight ahead (angle = 90 degree) for servo
00167     wait_ms (500);
00168     printf("\r\nDistance: %5.1f mm \n", sensor.getDistance_mm());
00169 
00170     init(); // initialise parameters (just for you to remember if you need to)
00171 
00172     wait_ms(1000); //wait 1 secs here before you go
00173     t.start();      // start timer (just demo of how you can use a timer)
00174 
00175     /*  ------------------
00176             with PID
00177         -----------------
00178     --  delete get_to_goal(); when you use without pid
00179     get_to_goal ();
00180     */
00181 
00182     /* ---------------------------
00183            without pid
00184        ------------------------
00185     */
00186     // delete between --- when you use it with pid
00187     // ------------------------------------------------------------
00188 /*
00189 =============================================================================
00190 Driving and stopping the robot with member functions from the wheel class
00191 =============================================================================
00192 */
00193     while(Dcenter() <= Dist_reach1())
00194     {
00195         robot.FW(0.5, 0.5);
00196         wait_ms(5000);
00197     }
00198     robot.stop(); // stop the robot again
00199     printf("\n\rtimeused = %4d tickL= %4d  tickR = %4d ", t.read_ms(),tickL,
00200            tickR);    
00201            
00202 } 
00203 /*  
00204 ----------------------------------------------------------------------------
00205                             END OF MAIN FUNCTION
00206 ----------------------------------------------------------------------------
00207 */
00208 
00209 /*
00210 =============================================================================
00211 Calculations of distances traveled by both wheels and robot
00212 =============================================================================
00213 */
00214 void tickLeft() // udtate left tick on tacho interrupt
00215 {
00216     tickL++;
00217 }
00218 void tickRight() // udtate right tick on tacho interrupt
00219 {
00220     tickR++;
00221 }
00222 float Dleft() // Distance for left wheel
00223 {
00224     return 2*PI*R*tickL/N;
00225 }
00226 
00227 float Dright() // Distance for right wheel
00228 {
00229     return 2*PI*R*tickR/N;
00230 }
00231 
00232 float Dcenter() // Distance for center
00233 {
00234     return (Dleft()+Dright())/2;
00235 }
00236 
00237 //Calculation of current position and new position with angles!!!
00238 
00239 float angle_new()
00240 {
00241     return (Dright()-Dleft())/L;   
00242 }
00243 
00244 float x_position()
00245 {
00246     return Dcenter()*sin(angle_new());   
00247 }
00248 
00249 float y_position()
00250 {
00251     return Dcenter()*cos(angle_new());
00252 }
00253 
00254 //slet efterfølgende
00255 float Dist_reach1() 
00256 { 
00257   double firstTosecond_calc = ((second_x-first_x)^2)+((second_y-first_y)^2);
00258   double dist_firstTosecond = sqrt(firstTosecond_calc); 
00259   
00260   return (dist_firstTosecond);
00261 }
00262 
00263 /*
00264 =============================================================================
00265 PID init
00266 =============================================================================
00267 */
00268 void get_to_goal ( ) // function to get to goal
00269 {
00270     double power = 0;
00271     double derivative = 0;
00272     double proportional = 0;
00273     double integral = 0;
00274     double e_old = 0;
00275 
00276     while (Dcenter() < Dist_reach1()) {
00277 /*
00278 -------------------------------------------------
00279 error is difference between right tick and left tick
00280 should be ajusted to your context, angle for robot
00281 and instead of test Dcenter() in your while loop, test
00282 for distance_to_goal()
00283 -------------------------------------------------
00284 */
00285         e = tickR-tickL; 
00286 /*
00287 =============================================================================
00288 PID regulator
00289 =============================================================================
00290 */
00291 
00292         // Compute the proportional
00293         proportional = e; // get the error
00294 
00295         // Compute the integral
00296         integral += proportional;
00297 
00298         // Compute the derivative
00299         derivative = e - e_old;
00300 
00301         // Remember the last error.
00302         e_old = e;
00303 
00304         // Compute the power
00305         power = (proportional * (P_TERM)) + (integral * (I_TERM))
00306                 + (derivative * (D_TERM));
00307 
00308         // Compute new speeds
00309 
00310         right = speed - power;//if power is 0, no error and same speed on wheels
00311         left = speed + power;
00312 
00313 
00314         // limit checks
00315         if (right < MIN)
00316             right = MIN;
00317         else if (right > MAX)
00318             right = MAX;
00319 
00320         if (left < MIN)
00321             left = MIN;
00322         else if (left > MAX)
00323             left = MAX;
00324 
00325         robot.FW(right,left); // set new positions
00326         printf("\n\r Dcenter = %3.2f tickL= %4d  tickR = %4d left %3.2f right %3.2f",
00327                Dcenter(),tickL,tickR,left,right);
00328 
00329         wait_ms(20); // should be adjusted to your context. Give the motor time
00330         // to do something before you react
00331     }
00332     t.stop(); // stop timer
00333 }
00334 
00335 void read_analog() // comes here every second in this case
00336 // could be flagged with global variables like "stop"
00337 {
00338     if(ain > 0.6f) { // 60% power, time for recharge
00339         dout = 1;
00340         stop=0;
00341     } else {
00342         dout = not dout;
00343         stop=1; // flash red led
00344     }
00345 
00346     if (ain==1.0f) greenout = 1; // full power
00347     else greenout = 0;
00348 }
00349 void init() // init your parameters
00350 {
00351     tickL=0;
00352     tickR=0;
00353 }