Kildekode
Fork of PRO1 by
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Thu Jul 14 2022 04:41:32 by
1.7.2
