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