Kildekode
Dependencies: mbed
Fork of Team5_kode by
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 }
Generated on Fri Jul 15 2022 02:11:27 by
1.7.2
