fdfdf
Dependencies: Motordriver mbed
Video of everything working: https://drive.google.com/a/usna.edu/file/d/0B6gx9qlkekGOSXVlQkN1RWpVcDg/view?usp=sharing
Updated Schematic
main.cpp@7:2bcff72b450b, 2015-02-04 (annotated)
- Committer:
- jared152
- Date:
- Wed Feb 04 18:10:03 2015 +0000
- Revision:
- 7:2bcff72b450b
- Parent:
- 6:d03601aced9b
- Child:
- 8:78f190f3a5aa
1. each range reading is now an average of 10 samples; 2. simplified multiple functions
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
jared152 | 0:e3a927f789f0 | 1 | // Jared Wilkins, Katie MacVarish, Grant LaTerza // |
jared152 | 0:e3a927f789f0 | 2 | // Capstone Project Code // |
jared152 | 0:e3a927f789f0 | 3 | /////////////////////////////////////////////////// |
jared152 | 0:e3a927f789f0 | 4 | |
jared152 | 0:e3a927f789f0 | 5 | #include "mbed.h" |
jared152 | 5:77b574b21fcc | 6 | #include "motordriver.h" //additional library to expand capabilities of PWMOut for use with motor drivers |
jared152 | 0:e3a927f789f0 | 7 | |
jared152 | 2:72f9b88d0e7c | 8 | //Initialization// |
jared152 | 0:e3a927f789f0 | 9 | Motor drive(p26, p25, p24, 1); //drive motor (PWM, fwd, rev, brake |
jared152 | 0:e3a927f789f0 | 10 | Motor wheel(p23, p22, p21, 1); //flywheel (PWM, fwd, rev, brake) |
jared152 | 2:72f9b88d0e7c | 11 | |
jared152 | 0:e3a927f789f0 | 12 | InterruptIn forward(p15); //remote signal to drive car forward |
jared152 | 0:e3a927f789f0 | 13 | InterruptIn reverse(p16); //signal to drive car in reverse |
jared152 | 2:72f9b88d0e7c | 14 | InterruptIn echo(p29); //interrupt for range echo pulse |
jared152 | 5:77b574b21fcc | 15 | |
jared152 | 2:72f9b88d0e7c | 16 | DigitalOut trigger(p30); //Dout fo the trigger |
jared152 | 2:72f9b88d0e7c | 17 | |
jared152 | 3:935aebd40f8a | 18 | Timer t; //initializes timer |
jared152 | 2:72f9b88d0e7c | 19 | |
jared152 | 1:f102ace95c79 | 20 | DigitalOut fwdled(LED1); //for testing interrupts, etc |
jared152 | 1:f102ace95c79 | 21 | DigitalOut revled(LED2); // "" |
jared152 | 5:77b574b21fcc | 22 | DigitalOut jumpled(LED3); // "" |
jared152 | 5:77b574b21fcc | 23 | DigitalOut testled(LED4); // "" |
jared152 | 0:e3a927f789f0 | 24 | |
jared152 | 1:f102ace95c79 | 25 | |
jared152 | 2:72f9b88d0e7c | 26 | int rev = 0, fwd = 0; //flag for fwd and rev |
jared152 | 7:2bcff72b450b | 27 | int t1, t2, //global variables for the echo pulse. c |
jared152 | 7:2bcff72b450b | 28 | t3, t4; //timer within flip |
jared152 | 2:72f9b88d0e7c | 29 | int jump = 0; //flag for wether or not car is jumping |
jared152 | 0:e3a927f789f0 | 30 | |
jared152 | 2:72f9b88d0e7c | 31 | //Interrupt Service Routines |
jared152 | 0:e3a927f789f0 | 32 | void driveforward() |
jared152 | 0:e3a927f789f0 | 33 | { |
jared152 | 0:e3a927f789f0 | 34 | fwd = 1; |
jared152 | 1:f102ace95c79 | 35 | rev = 0; |
jared152 | 0:e3a927f789f0 | 36 | } |
jared152 | 0:e3a927f789f0 | 37 | void drivereverse() |
jared152 | 0:e3a927f789f0 | 38 | { |
jared152 | 0:e3a927f789f0 | 39 | rev=1; |
jared152 | 1:f102ace95c79 | 40 | fwd = 0; |
jared152 | 0:e3a927f789f0 | 41 | } |
jared152 | 0:e3a927f789f0 | 42 | void stop() |
jared152 | 0:e3a927f789f0 | 43 | { |
jared152 | 0:e3a927f789f0 | 44 | fwd = rev = 0; |
jared152 | 0:e3a927f789f0 | 45 | } |
jared152 | 2:72f9b88d0e7c | 46 | void startechotimer() |
jared152 | 7:2bcff72b450b | 47 | { |
jared152 | 4:335d5a21f9f7 | 48 | t1 = t.read_us(); |
jared152 | 2:72f9b88d0e7c | 49 | } |
jared152 | 4:335d5a21f9f7 | 50 | void endechotimer() |
jared152 | 2:72f9b88d0e7c | 51 | { |
jared152 | 7:2bcff72b450b | 52 | t2 = t.read_us(); |
jared152 | 2:72f9b88d0e7c | 53 | } |
jared152 | 2:72f9b88d0e7c | 54 | |
jared152 | 7:2bcff72b450b | 55 | //Functions/////////////////// |
jared152 | 7:2bcff72b450b | 56 | |
jared152 | 7:2bcff72b450b | 57 | float read_ultrasonic(int number_of_samples) //function that takes returns an average of multiple distance samples |
jared152 | 2:72f9b88d0e7c | 58 | { |
jared152 | 7:2bcff72b450b | 59 | |
jared152 | 7:2bcff72b450b | 60 | float dist = 0.0; |
jared152 | 7:2bcff72b450b | 61 | |
jared152 | 7:2bcff72b450b | 62 | for (int i = 0; i< number_of_samples; i++) { |
jared152 | 7:2bcff72b450b | 63 | trigger = 1; // begin pulse |
jared152 | 7:2bcff72b450b | 64 | wait(.000015); // 15 us |
jared152 | 7:2bcff72b450b | 65 | trigger = 0; // end pulse |
jared152 | 7:2bcff72b450b | 66 | wait(.01); //wait for return pulse |
jared152 | 7:2bcff72b450b | 67 | dist+= (t2-t1)/58.0; //converts time to distance in cm |
jared152 | 7:2bcff72b450b | 68 | } |
jared152 | 7:2bcff72b450b | 69 | |
jared152 | 7:2bcff72b450b | 70 | dist = dist/number_of_samples; //take average |
jared152 | 2:72f9b88d0e7c | 71 | |
jared152 | 7:2bcff72b450b | 72 | return dist; |
jared152 | 7:2bcff72b450b | 73 | } //end distance() function |
jared152 | 7:2bcff72b450b | 74 | |
jared152 | 7:2bcff72b450b | 75 | void flip() //all flip function stuff in here |
jared152 | 2:72f9b88d0e7c | 76 | { |
jared152 | 7:2bcff72b450b | 77 | t3 = t.read(); |
jared152 | 7:2bcff72b450b | 78 | if (t4 - t3 < 6) { //ensures that this can only run once every six seconds |
jared152 | 7:2bcff72b450b | 79 | float PWM_wheel = 0.0; //initialize PWM speed for reactionwheel |
jared152 | 7:2bcff72b450b | 80 | PWM_wheel = 0.20; //***control law here*** |
jared152 | 7:2bcff72b450b | 81 | wheel.speed(PWM_wheel); //sets PWM for flywheel |
jared152 | 7:2bcff72b450b | 82 | t4 = t.read(); |
jared152 | 7:2bcff72b450b | 83 | } |
jared152 | 7:2bcff72b450b | 84 | } //end flip function |
jared152 | 2:72f9b88d0e7c | 85 | |
jared152 | 2:72f9b88d0e7c | 86 | |
jared152 | 2:72f9b88d0e7c | 87 | |
jared152 | 2:72f9b88d0e7c | 88 | //Main Program/////////////////////////// |
jared152 | 0:e3a927f789f0 | 89 | |
jared152 | 0:e3a927f789f0 | 90 | int main() |
jared152 | 0:e3a927f789f0 | 91 | { |
jared152 | 7:2bcff72b450b | 92 | |
jared152 | 2:72f9b88d0e7c | 93 | |
jared152 | 1:f102ace95c79 | 94 | float PWM_drive = 0.0; //initialize PWM speed for drive wheels |
jared152 | 7:2bcff72b450b | 95 | float range = 0.0; // variable to hold the range sensor value |
jared152 | 7:2bcff72b450b | 96 | |
jared152 | 5:77b574b21fcc | 97 | fwdled = 0; |
jared152 | 5:77b574b21fcc | 98 | revled = 0; |
jared152 | 5:77b574b21fcc | 99 | testled = 0; |
jared152 | 5:77b574b21fcc | 100 | jumpled = 0; |
jared152 | 0:e3a927f789f0 | 101 | |
jared152 | 1:f102ace95c79 | 102 | forward.rise(&driveforward); // rising edge on p15 triggers interrupt to go forward |
jared152 | 1:f102ace95c79 | 103 | forward.fall(&stop); // falling edge p15 triggers a stop |
jared152 | 0:e3a927f789f0 | 104 | |
jared152 | 1:f102ace95c79 | 105 | reverse.rise(&drivereverse); // rising edge p16 triggers go in reverse |
jared152 | 1:f102ace95c79 | 106 | reverse.fall(&stop); // falling edge p16 triggers stop |
jared152 | 1:f102ace95c79 | 107 | |
jared152 | 2:72f9b88d0e7c | 108 | echo.rise(&startechotimer); //rising edge of echo |
jared152 | 2:72f9b88d0e7c | 109 | echo.fall(&endechotimer); //falling edge of echo |
jared152 | 2:72f9b88d0e7c | 110 | |
jared152 | 2:72f9b88d0e7c | 111 | wheel.speed(0.0); // reaction wheel is initially off |
jared152 | 7:2bcff72b450b | 112 | drive.speed(0.0); //drive motor initially off |
jared152 | 2:72f9b88d0e7c | 113 | |
jared152 | 7:2bcff72b450b | 114 | t1 = t2 = t3 = t4 = 0; //initialize all timers variables to 0 |
jared152 | 7:2bcff72b450b | 115 | t.start(); //start timer |
jared152 | 7:2bcff72b450b | 116 | t1 = t2 = t3 = t4 = 0; //initialize all timers variables to 0 |
jared152 | 2:72f9b88d0e7c | 117 | |
jared152 | 1:f102ace95c79 | 118 | while(1) { |
jared152 | 7:2bcff72b450b | 119 | PWM_drive = 0.50; //****control law here for drive speed**** |
jared152 | 2:72f9b88d0e7c | 120 | |
jared152 | 7:2bcff72b450b | 121 | range = read_ultrasonic(10); //read distance for average of 10 samples |
jared152 | 7:2bcff72b450b | 122 | |
jared152 | 7:2bcff72b450b | 123 | if (range>6.0) { //when distance is greater than 6.0 cm it will do the flip function |
jared152 | 2:72f9b88d0e7c | 124 | flip(); |
jared152 | 3:935aebd40f8a | 125 | jumpled = 1; // lights LED 3 when in jump flag is true |
jared152 | 7:2bcff72b450b | 126 | } else { |
jared152 | 3:935aebd40f8a | 127 | jumpled = 0; // if not in jump mode led is off |
jared152 | 3:935aebd40f8a | 128 | } |
jared152 | 7:2bcff72b450b | 129 | |
jared152 | 1:f102ace95c79 | 130 | if(fwd) { //reads flags for drive/rev |
jared152 | 1:f102ace95c79 | 131 | drive.speed(PWM_drive); |
jared152 | 1:f102ace95c79 | 132 | fwdled = 1; //led1 on |
jared152 | 1:f102ace95c79 | 133 | revled = 0; //led2 off |
jared152 | 2:72f9b88d0e7c | 134 | } else if(rev) { |
jared152 | 0:e3a927f789f0 | 135 | drive.speed(-PWM_drive); |
jared152 | 1:f102ace95c79 | 136 | fwdled = 0; //led1 off |
jared152 | 1:f102ace95c79 | 137 | revled = 1; //led2 on |
jared152 | 1:f102ace95c79 | 138 | } else { |
jared152 | 0:e3a927f789f0 | 139 | drive.coast(); |
jared152 | 1:f102ace95c79 | 140 | fwdled = revled = 0; |
jared152 | 1:f102ace95c79 | 141 | } |
jared152 | 0:e3a927f789f0 | 142 | } |
jared152 | 0:e3a927f789f0 | 143 | } |