Kim Nielsen / Mbed 2 deprecated Endeligkildekode

Endelig kildekode med rettelser.

Dependencies:   PID mbed

Fork of EndeligKildekode by E2016-S1-Team5

robot.cpp@1:396a582e8861, 2016-10-31 (annotated)

Committer:
kimnielsen
Date:
Mon Oct 31 10:00:46 2016 +0000
Revision:
1:396a582e8861
Parent:
0:d3dbe632b1a9
Child:
2:1c27a43bb9b7
added double

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kimnielsen 0:d3dbe632b1a9 1 /*
kimnielsen 0:d3dbe632b1a9 2 ============================================================================
kimnielsen 0:d3dbe632b1a9 3 Name : robot.cpp
kimnielsen 0:d3dbe632b1a9 4 Author : Henning Slavensky
kimnielsen 0:d3dbe632b1a9 5 Version : 0.1
kimnielsen 0:d3dbe632b1a9 6 Date : 13102016
kimnielsen 0:d3dbe632b1a9 7 Copyright : Open for all
kimnielsen 0:d3dbe632b1a9 8 Description : Program to serve af platform for Pro1 2016
kimnielsen 0:d3dbe632b1a9 9 ============================================================================
kimnielsen 0:d3dbe632b1a9 10 */
kimnielsen 0:d3dbe632b1a9 11 #include "mbed.h"
kimnielsen 0:d3dbe632b1a9 12 #include "HCSR04.h"
kimnielsen 0:d3dbe632b1a9 13 #include "nucleo_servo.h"
kimnielsen 0:d3dbe632b1a9 14 #include "hack_motor.h"
kimnielsen 0:d3dbe632b1a9 15 #include <math.h> // nice when you got to work with real geometry stuff
kimnielsen 0:d3dbe632b1a9 16
kimnielsen 0:d3dbe632b1a9 17 #define PI 3.141592 // definition of PI
kimnielsen 0:d3dbe632b1a9 18
kimnielsen 0:d3dbe632b1a9 19 // PID definitions
kimnielsen 0:d3dbe632b1a9 20 #define P_TERM 0.1
kimnielsen 0:d3dbe632b1a9 21 #define I_TERM 0
kimnielsen 0:d3dbe632b1a9 22 #define D_TERM 0.0
kimnielsen 0:d3dbe632b1a9 23 #define MAX 1.0
kimnielsen 0:d3dbe632b1a9 24 #define MIN 0
kimnielsen 0:d3dbe632b1a9 25
kimnielsen 0:d3dbe632b1a9 26
kimnielsen 0:d3dbe632b1a9 27 /* ------------------------------
kimnielsen 0:d3dbe632b1a9 28 Definitions for robot
kimnielsen 0:d3dbe632b1a9 29 note that N,R,L maybe not have the most meaningful name
kimnielsen 0:d3dbe632b1a9 30 but they follow the names from the theory for our
kimnielsen 0:d3dbe632b1a9 31 robot lectures, and can be used in youa application
kimnielsen 0:d3dbe632b1a9 32 ------------------------------
kimnielsen 0:d3dbe632b1a9 33 */
kimnielsen 0:d3dbe632b1a9 34
kimnielsen 0:d3dbe632b1a9 35 #define N 20 // ticks on wheel
kimnielsen 0:d3dbe632b1a9 36 #define R 32.5 // radius = 32.5 mm
kimnielsen 0:d3dbe632b1a9 37 #define L 133 // 133 mm distance between wheels
kimnielsen 0:d3dbe632b1a9 38
kimnielsen 0:d3dbe632b1a9 39 //Coordinates for Robot with x and y
kimnielsen 0:d3dbe632b1a9 40 #define starting_x = 0
kimnielsen 0:d3dbe632b1a9 41 #define starting_y = 0
kimnielsen 0:d3dbe632b1a9 42 #define second_x = 5
kimnielsen 0:d3dbe632b1a9 43 #define second_y = 4
kimnielsen 0:d3dbe632b1a9 44 #define third_x = 7
kimnielsen 0:d3dbe632b1a9 45 #define third_y = 1
kimnielsen 0:d3dbe632b1a9 46
kimnielsen 1:396a582e8861 47 double distcalc(double, double)
kimnielsen 1:396a582e8861 48 {
kimnielsen 1:396a582e8861 49
kimnielsen 1:396a582e8861 50 }
kimnielsen 1:396a582e8861 51
kimnielsen 0:d3dbe632b1a9 52 Timer t; // define a timer t
kimnielsen 0:d3dbe632b1a9 53 Serial pc(USBTX, USBRX); // not used here because we only have one serial
kimnielsen 0:d3dbe632b1a9 54 // connection
kimnielsen 0:d3dbe632b1a9 55 InterruptIn tacho_left(PC_3); // Set PC_3 to be interupt in for tacho left
kimnielsen 0:d3dbe632b1a9 56 InterruptIn tacho_right(PC_2);// Set PC_2 to be interupt in for tacho right
kimnielsen 0:d3dbe632b1a9 57
kimnielsen 0:d3dbe632b1a9 58 Wheel robot(PB_2, PB_1, PB_15, PB_14); // create an object of robot H-bridge
kimnielsen 0:d3dbe632b1a9 59 // (M1A, M1B, M2A, M2B)
kimnielsen 0:d3dbe632b1a9 60
kimnielsen 0:d3dbe632b1a9 61 /* ---------------------------
kimnielsen 0:d3dbe632b1a9 62 definition for analog power
kimnielsen 0:d3dbe632b1a9 63 ---------------------------
kimnielsen 0:d3dbe632b1a9 64 */
kimnielsen 0:d3dbe632b1a9 65 AnalogIn ain(PC_4);
kimnielsen 0:d3dbe632b1a9 66 DigitalOut dout(PB_13);
kimnielsen 0:d3dbe632b1a9 67 DigitalOut greenout(PB_12);
kimnielsen 0:d3dbe632b1a9 68
kimnielsen 0:d3dbe632b1a9 69 int stop=0;
kimnielsen 0:d3dbe632b1a9 70
kimnielsen 0:d3dbe632b1a9 71 /* ---------------------
kimnielsen 0:d3dbe632b1a9 72 Global variables
kimnielsen 0:d3dbe632b1a9 73 --------------------
kimnielsen 0:d3dbe632b1a9 74 */
kimnielsen 0:d3dbe632b1a9 75 double right,left;
kimnielsen 0:d3dbe632b1a9 76 double speed=0.5;
kimnielsen 0:d3dbe632b1a9 77 double e = 0; // angle error
kimnielsen 0:d3dbe632b1a9 78 int tickL = 0; // tick on left wheel
kimnielsen 0:d3dbe632b1a9 79 int tickR = 0; // tick on right wheel
kimnielsen 0:d3dbe632b1a9 80
kimnielsen 0:d3dbe632b1a9 81 /* -----------
kimnielsen 0:d3dbe632b1a9 82 Prototypes
kimnielsen 0:d3dbe632b1a9 83 -----------
kimnielsen 0:d3dbe632b1a9 84 */
kimnielsen 0:d3dbe632b1a9 85 void init();
kimnielsen 0:d3dbe632b1a9 86 void tickLeft(); // read tick left
kimnielsen 0:d3dbe632b1a9 87 void tickRight(); // read tick right
kimnielsen 0:d3dbe632b1a9 88 float Dleft(); // distance left wheel
kimnielsen 0:d3dbe632b1a9 89 float Dright(); // distance right wheel
kimnielsen 0:d3dbe632b1a9 90 float Dcenter(); // Distance for center
kimnielsen 0:d3dbe632b1a9 91 void get_to_goal ( ); // function to get to goal
kimnielsen 0:d3dbe632b1a9 92 void read_analog(); // comes here every second
kimnielsen 0:d3dbe632b1a9 93
kimnielsen 0:d3dbe632b1a9 94 Ticker T1; // create an object T1 of Ticker
kimnielsen 0:d3dbe632b1a9 95
kimnielsen 0:d3dbe632b1a9 96 int main()
kimnielsen 0:d3dbe632b1a9 97 {
kimnielsen 0:d3dbe632b1a9 98
kimnielsen 0:d3dbe632b1a9 99 T1.attach(&read_analog, 1.0); // attach the address of the read_analog
kimnielsen 0:d3dbe632b1a9 100 //function to read analog in every second
kimnielsen 0:d3dbe632b1a9 101
kimnielsen 0:d3dbe632b1a9 102 tacho_left.rise(&tickLeft); // attach the address of the count function
kimnielsen 0:d3dbe632b1a9 103 //to the falling edge
kimnielsen 0:d3dbe632b1a9 104 tacho_right.rise(&tickRight); // attach the address of the count function
kimnielsen 0:d3dbe632b1a9 105 //to the falling edge
kimnielsen 0:d3dbe632b1a9 106
kimnielsen 0:d3dbe632b1a9 107 HCSR04 sensor(PC_5,PC_6); // Create an object of HCSR04 ultrasonic with pin
kimnielsen 0:d3dbe632b1a9 108 // (echo,trigger) on pin PC_5, PC6
kimnielsen 0:d3dbe632b1a9 109 Servo servo1(PC_8); //Create an object of Servo class on pin PC_8
kimnielsen 0:d3dbe632b1a9 110
kimnielsen 0:d3dbe632b1a9 111 sensor.setRanges(2, 400); // set the range for Ultrasonic
kimnielsen 0:d3dbe632b1a9 112
kimnielsen 0:d3dbe632b1a9 113 /* -----------------------------------------
kimnielsen 0:d3dbe632b1a9 114 Demo of the servor and ulta sonic sensor
kimnielsen 0:d3dbe632b1a9 115 -----------------------------------------
kimnielsen 0:d3dbe632b1a9 116 */
kimnielsen 0:d3dbe632b1a9 117 wait_ms(2000); // just get time for you to enable your screeen
kimnielsen 0:d3dbe632b1a9 118 servo1.set_position(0); // Servo right position (angle = 0 degree) for servo
kimnielsen 0:d3dbe632b1a9 119 wait_ms (500);
kimnielsen 0:d3dbe632b1a9 120 printf("\r\nDistance: %5.1f mm", sensor.getDistance_mm()); // display the
kimnielsen 0:d3dbe632b1a9 121 //readings from ultra sonic at this position
kimnielsen 0:d3dbe632b1a9 122 servo1.set_position(180); // Servo left position (angle = 180 degree)
kimnielsen 0:d3dbe632b1a9 123 //for servo
kimnielsen 0:d3dbe632b1a9 124 wait_ms (500);
kimnielsen 0:d3dbe632b1a9 125 printf("\r\nDistance: %5.1f mm", sensor.getDistance_mm());
kimnielsen 0:d3dbe632b1a9 126 servo1.set_position(90); // Straight ahead (angle = 90 degree) for servo
kimnielsen 0:d3dbe632b1a9 127 wait_ms (500);
kimnielsen 0:d3dbe632b1a9 128 printf("\r\nDistance: %5.1f mm", sensor.getDistance_mm());
kimnielsen 0:d3dbe632b1a9 129
kimnielsen 0:d3dbe632b1a9 130 init(); // initialise parameters (just for you to remember if you need to)
kimnielsen 0:d3dbe632b1a9 131
kimnielsen 0:d3dbe632b1a9 132 wait_ms(1000); //wait 1 secs here before you go
kimnielsen 0:d3dbe632b1a9 133 t.start(); // start timer (just demo of how you can use a timer)
kimnielsen 0:d3dbe632b1a9 134
kimnielsen 0:d3dbe632b1a9 135 /* ------------------
kimnielsen 0:d3dbe632b1a9 136 with PID
kimnielsen 0:d3dbe632b1a9 137 -----------------
kimnielsen 0:d3dbe632b1a9 138 -- delete get_to_goal(); when you use without pid
kimnielsen 0:d3dbe632b1a9 139 get_to_goal ();
kimnielsen 0:d3dbe632b1a9 140 */
kimnielsen 0:d3dbe632b1a9 141
kimnielsen 0:d3dbe632b1a9 142 /* ---------------------------
kimnielsen 0:d3dbe632b1a9 143 without pid
kimnielsen 0:d3dbe632b1a9 144 ------------------------
kimnielsen 0:d3dbe632b1a9 145 */
kimnielsen 0:d3dbe632b1a9 146 // delete between --- when you use it with pid
kimnielsen 0:d3dbe632b1a9 147 // ------------------------------------------------------------
kimnielsen 0:d3dbe632b1a9 148 while (Dcenter() <= CLOSE_ENOUGH) { // while distance traveled is less than
kimnielsen 0:d3dbe632b1a9 149 //target
kimnielsen 0:d3dbe632b1a9 150
kimnielsen 0:d3dbe632b1a9 151 robot.FW(0.5,0.5); // set new values half speed on both wheels
kimnielsen 0:d3dbe632b1a9 152 wait_ms(20);
kimnielsen 0:d3dbe632b1a9 153 }
kimnielsen 0:d3dbe632b1a9 154 // --------------------------------------------------------------
kimnielsen 0:d3dbe632b1a9 155
kimnielsen 0:d3dbe632b1a9 156 robot.stop(); // stop the robot again
kimnielsen 0:d3dbe632b1a9 157 printf("\n\rtimeused = %4d tickL= %4d tickR = %4d ", t.read_ms(),tickL,
kimnielsen 0:d3dbe632b1a9 158 tickR);
kimnielsen 0:d3dbe632b1a9 159
kimnielsen 0:d3dbe632b1a9 160 }
kimnielsen 0:d3dbe632b1a9 161
kimnielsen 0:d3dbe632b1a9 162 void tickLeft() // udtate left tick on tacho interrupt
kimnielsen 0:d3dbe632b1a9 163 {
kimnielsen 0:d3dbe632b1a9 164 tickL++;
kimnielsen 0:d3dbe632b1a9 165 }
kimnielsen 0:d3dbe632b1a9 166 void tickRight() // udtate right tick on tacho interrupt
kimnielsen 0:d3dbe632b1a9 167 {
kimnielsen 0:d3dbe632b1a9 168 tickR++;
kimnielsen 0:d3dbe632b1a9 169 }
kimnielsen 0:d3dbe632b1a9 170 float Dleft() // Distance for left wheel
kimnielsen 0:d3dbe632b1a9 171 {
kimnielsen 0:d3dbe632b1a9 172
kimnielsen 0:d3dbe632b1a9 173 return 2*PI*R*tickL/N;
kimnielsen 0:d3dbe632b1a9 174 }
kimnielsen 0:d3dbe632b1a9 175
kimnielsen 0:d3dbe632b1a9 176 float Dright() // Distance for right wheel
kimnielsen 0:d3dbe632b1a9 177 {
kimnielsen 0:d3dbe632b1a9 178
kimnielsen 0:d3dbe632b1a9 179 return 2*PI*R*tickR/N;
kimnielsen 0:d3dbe632b1a9 180 }
kimnielsen 0:d3dbe632b1a9 181
kimnielsen 0:d3dbe632b1a9 182 float Dcenter() // Distance for center
kimnielsen 0:d3dbe632b1a9 183 {
kimnielsen 0:d3dbe632b1a9 184 return (Dleft()+Dright())/2;
kimnielsen 0:d3dbe632b1a9 185 }
kimnielsen 0:d3dbe632b1a9 186
kimnielsen 0:d3dbe632b1a9 187
kimnielsen 0:d3dbe632b1a9 188 void get_to_goal ( ) // function to get to goal
kimnielsen 0:d3dbe632b1a9 189 {
kimnielsen 0:d3dbe632b1a9 190
kimnielsen 0:d3dbe632b1a9 191 // PID init
kimnielsen 0:d3dbe632b1a9 192 double power, derivative, proportional, integral, e_old;
kimnielsen 0:d3dbe632b1a9 193 power=derivative=proportional=integral=e_old=0;
kimnielsen 0:d3dbe632b1a9 194
kimnielsen 0:d3dbe632b1a9 195 while (Dcenter() < CLOSE_ENOUGH) {
kimnielsen 0:d3dbe632b1a9 196
kimnielsen 0:d3dbe632b1a9 197 e = tickR-tickL; // error is difference between right tick and left tick
kimnielsen 0:d3dbe632b1a9 198 // should be ajusted to your context, angle for robot
kimnielsen 0:d3dbe632b1a9 199 // and instead of test Dcenter() in your while loop, test
kimnielsen 0:d3dbe632b1a9 200 // for distance_to_goal()
kimnielsen 0:d3dbe632b1a9 201
kimnielsen 0:d3dbe632b1a9 202 /* --------------------------
kimnielsen 0:d3dbe632b1a9 203 * PID regulator
kimnielsen 0:d3dbe632b1a9 204 * --------------------------
kimnielsen 0:d3dbe632b1a9 205 */
kimnielsen 0:d3dbe632b1a9 206
kimnielsen 0:d3dbe632b1a9 207 // Compute the proportional
kimnielsen 0:d3dbe632b1a9 208 proportional = e; // get the error
kimnielsen 0:d3dbe632b1a9 209
kimnielsen 0:d3dbe632b1a9 210 // Compute the integral
kimnielsen 0:d3dbe632b1a9 211 integral += proportional;
kimnielsen 0:d3dbe632b1a9 212
kimnielsen 0:d3dbe632b1a9 213 // Compute the derivative
kimnielsen 0:d3dbe632b1a9 214 derivative = e - e_old;
kimnielsen 0:d3dbe632b1a9 215
kimnielsen 0:d3dbe632b1a9 216 // Remember the last error.
kimnielsen 0:d3dbe632b1a9 217 e_old = e;
kimnielsen 0:d3dbe632b1a9 218
kimnielsen 0:d3dbe632b1a9 219 // Compute the power
kimnielsen 0:d3dbe632b1a9 220 power = (proportional * (P_TERM)) + (integral * (I_TERM))
kimnielsen 0:d3dbe632b1a9 221 + (derivative * (D_TERM));
kimnielsen 0:d3dbe632b1a9 222
kimnielsen 0:d3dbe632b1a9 223 // Compute new speeds
kimnielsen 0:d3dbe632b1a9 224
kimnielsen 0:d3dbe632b1a9 225 right = speed - power;//if power is 0, no error and same speed on wheels
kimnielsen 0:d3dbe632b1a9 226 left = speed + power;
kimnielsen 0:d3dbe632b1a9 227
kimnielsen 0:d3dbe632b1a9 228
kimnielsen 0:d3dbe632b1a9 229 // limit checks
kimnielsen 0:d3dbe632b1a9 230 if (right < MIN)
kimnielsen 0:d3dbe632b1a9 231 right = MIN;
kimnielsen 0:d3dbe632b1a9 232 else if (right > MAX)
kimnielsen 0:d3dbe632b1a9 233 right = MAX;
kimnielsen 0:d3dbe632b1a9 234
kimnielsen 0:d3dbe632b1a9 235 if (left < MIN)
kimnielsen 0:d3dbe632b1a9 236 left = MIN;
kimnielsen 0:d3dbe632b1a9 237 else if (left > MAX)
kimnielsen 0:d3dbe632b1a9 238 left = MAX;
kimnielsen 0:d3dbe632b1a9 239
kimnielsen 0:d3dbe632b1a9 240 robot.FW(right,left); // set new positions
kimnielsen 0:d3dbe632b1a9 241 printf("\n\r Dcenter = %3.2f tickL= %4d tickR = %4d left %3.2f right %3.2f",
kimnielsen 0:d3dbe632b1a9 242 Dcenter(),tickL,tickR,left,right);
kimnielsen 0:d3dbe632b1a9 243
kimnielsen 0:d3dbe632b1a9 244 wait_ms(20); // should be adusted to your context. Give the motor time
kimnielsen 0:d3dbe632b1a9 245 // to do something before you react
kimnielsen 0:d3dbe632b1a9 246
kimnielsen 0:d3dbe632b1a9 247 }
kimnielsen 0:d3dbe632b1a9 248
kimnielsen 0:d3dbe632b1a9 249 t.stop(); // stop timer
kimnielsen 0:d3dbe632b1a9 250
kimnielsen 0:d3dbe632b1a9 251 }
kimnielsen 0:d3dbe632b1a9 252
kimnielsen 0:d3dbe632b1a9 253 void read_analog() // comes here every second in this case
kimnielsen 0:d3dbe632b1a9 254 // could be flagged with global variables like "stop"
kimnielsen 0:d3dbe632b1a9 255 {
kimnielsen 0:d3dbe632b1a9 256 if(ain > 0.6f) { // 60% power, time for recharge
kimnielsen 0:d3dbe632b1a9 257 dout = 1;
kimnielsen 0:d3dbe632b1a9 258 stop=0;
kimnielsen 0:d3dbe632b1a9 259 } else {
kimnielsen 0:d3dbe632b1a9 260 dout = not dout;
kimnielsen 0:d3dbe632b1a9 261 stop=1; // flash red led
kimnielsen 0:d3dbe632b1a9 262 }
kimnielsen 0:d3dbe632b1a9 263
kimnielsen 0:d3dbe632b1a9 264 if (ain==1.0f) greenout = 1; // full power
kimnielsen 0:d3dbe632b1a9 265 else greenout = 0;
kimnielsen 0:d3dbe632b1a9 266 }
kimnielsen 0:d3dbe632b1a9 267 void init() // init your parameters
kimnielsen 0:d3dbe632b1a9 268 {
kimnielsen 0:d3dbe632b1a9 269 tickL=0;
kimnielsen 0:d3dbe632b1a9 270 tickR=0;
kimnielsen 0:d3dbe632b1a9 271 }