Kim Nielsen / Mbed 2 deprecated Endeligkildekode

Endelig kildekode med rettelser.

Dependencies:   PID mbed

Fork of EndeligKildekode by E2016-S1-Team5

robot.cpp@0:d3dbe632b1a9, 2016-10-31 (annotated)

Committer:
kimnielsen
Date:
Mon Oct 31 09:57:07 2016 +0000
Revision:
0:d3dbe632b1a9
Child:
1:396a582e8861
Hej

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