Jared Wilkins / Mbed 2 deprecated Capstone

fdfdf

Dependencies:   Motordriver mbed

Video of everything working: https://drive.google.com/a/usna.edu/file/d/0B6gx9qlkekGOSXVlQkN1RWpVcDg/view?usp=sharing

Updated Schematic /media/uploads/jared152/capstone_schematic.png

main.cpp@13:7e7c97f73ace, 2015-02-05 (annotated)

Committer:
jared152
Date:
Thu Feb 05 03:13:46 2015 +0000
Revision:
13:7e7c97f73ace
Parent:
12:008016f03ca1
Child:
14:931c5abf0006
put drive PWM controls in the interrupts and removed flags

Who changed what in which revision?

UserRevisionLine numberNew 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 13:7e7c97f73ace 9 Motor wheel(p24, p26, p25, 1); //flywheel (PWM, fwd, rev, brake)
jared152 13:7e7c97f73ace 10 Motor drive(p21, p23, p22, 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 13:7e7c97f73ace 26 //Global Variables
jared152 13:7e7c97f73ace 27 int jump = 0; //flag for whether jumping or not
jared152 9:fe3a1e9b08b7 28 int t1, t2, //global variables for the echo pulse. c
jared152 13:7e7c97f73ace 29 t3, t4; //timer variables within flip function
jared152 13:7e7c97f73ace 30 float PWM_drive = 0.0; //initialize PWM speed for drive wheels
jared152 13:7e7c97f73ace 31
jared152 10:0ee2d6d8d965 32
jared152 9:fe3a1e9b08b7 33 //Interrupt Service Routines////////////////
jared152 0:e3a927f789f0 34 void driveforward()
jared152 0:e3a927f789f0 35 {
jared152 13:7e7c97f73ace 36 fwdled = 1;
jared152 13:7e7c97f73ace 37 revled = 0;
jared152 13:7e7c97f73ace 38 drive.speed(PWM_drive);
jared152 0:e3a927f789f0 39 }
jared152 0:e3a927f789f0 40 void drivereverse()
jared152 0:e3a927f789f0 41 {
jared152 13:7e7c97f73ace 42 revled =1;
jared152 13:7e7c97f73ace 43 fwdled = 0;
jared152 13:7e7c97f73ace 44 drive.speed(-PWM_drive);
jared152 0:e3a927f789f0 45 }
jared152 0:e3a927f789f0 46 void stop()
jared152 0:e3a927f789f0 47 {
jared152 13:7e7c97f73ace 48 fwdled = revled = 0;
jared152 13:7e7c97f73ace 49 drive.coast();
jared152 0:e3a927f789f0 50 }
jared152 2:72f9b88d0e7c 51 void startechotimer()
jared152 7:2bcff72b450b 52 {
jared152 4:335d5a21f9f7 53 t1 = t.read_us();
jared152 2:72f9b88d0e7c 54 }
jared152 4:335d5a21f9f7 55 void endechotimer()
jared152 2:72f9b88d0e7c 56 {
jared152 7:2bcff72b450b 57 t2 = t.read_us();
jared152 2:72f9b88d0e7c 58 }
jared152 2:72f9b88d0e7c 59
jared152 9:fe3a1e9b08b7 60 //Functions//////////////////////////
jared152 7:2bcff72b450b 61
jared152 7:2bcff72b450b 62 float read_ultrasonic(int number_of_samples) //function that takes returns an average of multiple distance samples
jared152 2:72f9b88d0e7c 63 {
jared152 7:2bcff72b450b 64
jared152 7:2bcff72b450b 65 float dist = 0.0;
jared152 7:2bcff72b450b 66
jared152 7:2bcff72b450b 67 for (int i = 0; i< number_of_samples; i++) {
jared152 7:2bcff72b450b 68 trigger = 1; // begin pulse
jared152 7:2bcff72b450b 69 wait(.000015); // 15 us
jared152 7:2bcff72b450b 70 trigger = 0; // end pulse
jared152 10:0ee2d6d8d965 71 //wait(.001); //wait for return pulse
jared152 7:2bcff72b450b 72 dist+= (t2-t1)/58.0; //converts time to distance in cm
jared152 7:2bcff72b450b 73 }
jared152 7:2bcff72b450b 74
jared152 7:2bcff72b450b 75 dist = dist/number_of_samples; //take average
jared152 2:72f9b88d0e7c 76
jared152 7:2bcff72b450b 77 return dist;
jared152 7:2bcff72b450b 78 } //end distance() function
jared152 7:2bcff72b450b 79
jared152 7:2bcff72b450b 80 void flip() //all flip function stuff in here
jared152 2:72f9b88d0e7c 81 {
jared152 12:008016f03ca1 82 float PWM_wheel = 0.0; //initialize PWM speed for reactionwheel
jared152 7:2bcff72b450b 83 t3 = t.read();
jared152 12:008016f03ca1 84 if (t3 - t4 >= 5) { //ensures that this can only run once every six seconds
jared152 10:0ee2d6d8d965 85 t3 = t.read();
jared152 13:7e7c97f73ace 86 testled = 1; // turns on led 3 (during flip)
jared152 12:008016f03ca1 87
jared152 13:7e7c97f73ace 88 PWM_wheel = 1.0; //***control law here***
jared152 13:7e7c97f73ace 89 wheel.speed(PWM_wheel); //sets PWM for flywheel
jared152 13:7e7c97f73ace 90 wait(2); //settling time
jared152 13:7e7c97f73ace 91 wheel.stop(1.0); //regenerative braking
jared152 12:008016f03ca1 92
jared152 7:2bcff72b450b 93 t4 = t.read();
jared152 13:7e7c97f73ace 94 testled = 0; // turn off led (not flipping)
jared152 10:0ee2d6d8d965 95 } else
jared152 10:0ee2d6d8d965 96 testled = 0;
jared152 10:0ee2d6d8d965 97
jared152 10:0ee2d6d8d965 98
jared152 7:2bcff72b450b 99 } //end flip function
jared152 2:72f9b88d0e7c 100
jared152 2:72f9b88d0e7c 101
jared152 2:72f9b88d0e7c 102
jared152 2:72f9b88d0e7c 103 //Main Program///////////////////////////
jared152 0:e3a927f789f0 104
jared152 0:e3a927f789f0 105 int main()
jared152 0:e3a927f789f0 106 {
jared152 7:2bcff72b450b 107 float range = 0.0; // variable to hold the range sensor value
jared152 10:0ee2d6d8d965 108
jared152 13:7e7c97f73ace 109 fwdled = revled = testled = jumpled = 0; //intializes all LEDS to off
jared152 13:7e7c97f73ace 110
jared152 0:e3a927f789f0 111
jared152 1:f102ace95c79 112 forward.rise(&driveforward); // rising edge on p15 triggers interrupt to go forward
jared152 1:f102ace95c79 113 forward.fall(&stop); // falling edge p15 triggers a stop
jared152 0:e3a927f789f0 114
jared152 1:f102ace95c79 115 reverse.rise(&drivereverse); // rising edge p16 triggers go in reverse
jared152 1:f102ace95c79 116 reverse.fall(&stop); // falling edge p16 triggers stop
jared152 1:f102ace95c79 117
jared152 2:72f9b88d0e7c 118 echo.rise(&startechotimer); //rising edge of echo
jared152 2:72f9b88d0e7c 119 echo.fall(&endechotimer); //falling edge of echo
jared152 2:72f9b88d0e7c 120
jared152 2:72f9b88d0e7c 121 wheel.speed(0.0); // reaction wheel is initially off
jared152 7:2bcff72b450b 122 drive.speed(0.0); //drive motor initially off
jared152 2:72f9b88d0e7c 123
jared152 10:0ee2d6d8d965 124 t1 = t2 = t4 = 0; //initialize all timers variables to 0
jared152 10:0ee2d6d8d965 125 t3 = 10;
jared152 7:2bcff72b450b 126 t.start(); //start timer
jared152 2:72f9b88d0e7c 127
jared152 1:f102ace95c79 128 while(1) {
jared152 12:008016f03ca1 129 PWM_drive = 1.0; //****control law here for drive speed****
jared152 2:72f9b88d0e7c 130
jared152 13:7e7c97f73ace 131 range = read_ultrasonic(20); //read distance for average of 20 samples
jared152 7:2bcff72b450b 132
jared152 10:0ee2d6d8d965 133 if (range > 6.0) { //when distance is greater than 6.0 cm it will do the flip function
jared152 10:0ee2d6d8d965 134 jumpled = 1; // lights LED 3 when in jump flag is true
jared152 10:0ee2d6d8d965 135 flip();
jared152 7:2bcff72b450b 136 } else {
jared152 3:935aebd40f8a 137 jumpled = 0; // if not in jump mode led is off
jared152 3:935aebd40f8a 138 }
jared152 13:7e7c97f73ace 139
jared152 0:e3a927f789f0 140 }
jared152 0:e3a927f789f0 141 }