Program to control an accelerometer, motors and a rangefinder using the ScmRTOS ported to mbed. (Work in progress and buggy)
Hexacopter/PING_rangefinder.h@0:9b057566f9ee, 2010-11-01 (annotated)
- Committer:
- jberry
- Date:
- Mon Nov 01 20:39:01 2010 +0000
- Revision:
- 0:9b057566f9ee
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
jberry | 0:9b057566f9ee | 1 | #pragma once |
jberry | 0:9b057566f9ee | 2 | #ifndef PING_RANGEFINDER_H |
jberry | 0:9b057566f9ee | 3 | #define PING_RANGEFINDER_H |
jberry | 0:9b057566f9ee | 4 | |
jberry | 0:9b057566f9ee | 5 | #include <processes.h> |
jberry | 0:9b057566f9ee | 6 | #include <Mutexes.h> |
jberry | 0:9b057566f9ee | 7 | #include <channels.h> |
jberry | 0:9b057566f9ee | 8 | #define PINGPIN p22 |
jberry | 0:9b057566f9ee | 9 | |
jberry | 0:9b057566f9ee | 10 | |
jberry | 0:9b057566f9ee | 11 | //DigitalOut led3(LED3); //these LEDs are used to indicate when an interrupt occurs. |
jberry | 0:9b057566f9ee | 12 | //DigitalOut led4(LED4); |
jberry | 0:9b057566f9ee | 13 | //byte count = 0; |
jberry | 0:9b057566f9ee | 14 | |
jberry | 0:9b057566f9ee | 15 | class rangefinder { |
jberry | 0:9b057566f9ee | 16 | public: |
jberry | 0:9b057566f9ee | 17 | rangefinder() |
jberry | 0:9b057566f9ee | 18 | :Pinterrupt(PINGPIN), PingPin(PINGPIN) |
jberry | 0:9b057566f9ee | 19 | { |
jberry | 0:9b057566f9ee | 20 | //led3 = ~(led4 = 0); |
jberry | 0:9b057566f9ee | 21 | |
jberry | 0:9b057566f9ee | 22 | time = 0; |
jberry | 0:9b057566f9ee | 23 | enable_flag = false; |
jberry | 0:9b057566f9ee | 24 | set_interrupts(); |
jberry | 0:9b057566f9ee | 25 | PingPin.mode(PullDown); |
jberry | 0:9b057566f9ee | 26 | //Pinterrupt.mode(PullDown); |
jberry | 0:9b057566f9ee | 27 | // Pinterrupt.mode(PullUp); |
jberry | 0:9b057566f9ee | 28 | PingPin.output(); //set pingpin as output |
jberry | 0:9b057566f9ee | 29 | PingPin = 0; |
jberry | 0:9b057566f9ee | 30 | } |
jberry | 0:9b057566f9ee | 31 | |
jberry | 0:9b057566f9ee | 32 | void ping() { |
jberry | 0:9b057566f9ee | 33 | T.reset(); //reset the timer to 0 |
jberry | 0:9b057566f9ee | 34 | set_interrupts(); |
jberry | 0:9b057566f9ee | 35 | enable_flag = false; |
jberry | 0:9b057566f9ee | 36 | PingPin.output(); //set as output |
jberry | 0:9b057566f9ee | 37 | PingPin = 0; //output initiation sequence of 0 1 0 to the rangefinder |
jberry | 0:9b057566f9ee | 38 | wait_us(2); |
jberry | 0:9b057566f9ee | 39 | PingPin = 1; |
jberry | 0:9b057566f9ee | 40 | wait_us(5); |
jberry | 0:9b057566f9ee | 41 | PingPin = 0; |
jberry | 0:9b057566f9ee | 42 | wait_us(1); |
jberry | 0:9b057566f9ee | 43 | //printf("now waiting for input.\n"); |
jberry | 0:9b057566f9ee | 44 | PingPin.input(); //set as input |
jberry | 0:9b057566f9ee | 45 | wait_us(10); |
jberry | 0:9b057566f9ee | 46 | //set_interrupts(); |
jberry | 0:9b057566f9ee | 47 | enable_flag = true; |
jberry | 0:9b057566f9ee | 48 | wait_ms(25); //more than required for maximum distance measurement |
jberry | 0:9b057566f9ee | 49 | |
jberry | 0:9b057566f9ee | 50 | |
jberry | 0:9b057566f9ee | 51 | |
jberry | 0:9b057566f9ee | 52 | //T.start(); //begin timing the length of time for an interrupt to occur |
jberry | 0:9b057566f9ee | 53 | } |
jberry | 0:9b057566f9ee | 54 | void set_interrupts() |
jberry | 0:9b057566f9ee | 55 | { |
jberry | 0:9b057566f9ee | 56 | Pinterrupt.fall(this, &rangefinder::set_time); |
jberry | 0:9b057566f9ee | 57 | Pinterrupt.rise(this, &rangefinder::start_timing); |
jberry | 0:9b057566f9ee | 58 | } |
jberry | 0:9b057566f9ee | 59 | void disable_interrupts() |
jberry | 0:9b057566f9ee | 60 | { |
jberry | 0:9b057566f9ee | 61 | Pinterrupt.fall(NULL); |
jberry | 0:9b057566f9ee | 62 | Pinterrupt.rise(NULL); |
jberry | 0:9b057566f9ee | 63 | |
jberry | 0:9b057566f9ee | 64 | } |
jberry | 0:9b057566f9ee | 65 | void start_timing() { OS::TISRW ISRW; |
jberry | 0:9b057566f9ee | 66 | if(enable_flag == true) |
jberry | 0:9b057566f9ee | 67 | { |
jberry | 0:9b057566f9ee | 68 | T.start(); |
jberry | 0:9b057566f9ee | 69 | //led3 = !led3; |
jberry | 0:9b057566f9ee | 70 | //count++; |
jberry | 0:9b057566f9ee | 71 | } |
jberry | 0:9b057566f9ee | 72 | |
jberry | 0:9b057566f9ee | 73 | //printf("start_timing called.\n"); |
jberry | 0:9b057566f9ee | 74 | } |
jberry | 0:9b057566f9ee | 75 | |
jberry | 0:9b057566f9ee | 76 | void set_time() { OS::TISRW ISRW; |
jberry | 0:9b057566f9ee | 77 | if(enable_flag == true) |
jberry | 0:9b057566f9ee | 78 | { |
jberry | 0:9b057566f9ee | 79 | T.stop(); |
jberry | 0:9b057566f9ee | 80 | //led4 = !led4; |
jberry | 0:9b057566f9ee | 81 | //disable_interrupts(); |
jberry | 0:9b057566f9ee | 82 | //enable_flag = false; |
jberry | 0:9b057566f9ee | 83 | //printf("set_time being called...\n"); |
jberry | 0:9b057566f9ee | 84 | time = T.read_us(); |
jberry | 0:9b057566f9ee | 85 | //count++; |
jberry | 0:9b057566f9ee | 86 | } |
jberry | 0:9b057566f9ee | 87 | //printf("value stored in time %f: \n", time); |
jberry | 0:9b057566f9ee | 88 | |
jberry | 0:9b057566f9ee | 89 | //PingPin.output(); |
jberry | 0:9b057566f9ee | 90 | //PingPin = 0; |
jberry | 0:9b057566f9ee | 91 | //printf("PING:%f\n",get_time()); |
jberry | 0:9b057566f9ee | 92 | |
jberry | 0:9b057566f9ee | 93 | } |
jberry | 0:9b057566f9ee | 94 | |
jberry | 0:9b057566f9ee | 95 | |
jberry | 0:9b057566f9ee | 96 | |
jberry | 0:9b057566f9ee | 97 | float get_time() { |
jberry | 0:9b057566f9ee | 98 | //disable_interrupts(); |
jberry | 0:9b057566f9ee | 99 | return time/2.0; |
jberry | 0:9b057566f9ee | 100 | |
jberry | 0:9b057566f9ee | 101 | } |
jberry | 0:9b057566f9ee | 102 | |
jberry | 0:9b057566f9ee | 103 | |
jberry | 0:9b057566f9ee | 104 | private: |
jberry | 0:9b057566f9ee | 105 | Timer T; |
jberry | 0:9b057566f9ee | 106 | float time; //time in us |
jberry | 0:9b057566f9ee | 107 | InterruptIn Pinterrupt; |
jberry | 0:9b057566f9ee | 108 | DigitalInOut PingPin; |
jberry | 0:9b057566f9ee | 109 | bool enable_flag; |
jberry | 0:9b057566f9ee | 110 | //Ticker TICKER;// send_ticker; |
jberry | 0:9b057566f9ee | 111 | |
jberry | 0:9b057566f9ee | 112 | |
jberry | 0:9b057566f9ee | 113 | }; |
jberry | 0:9b057566f9ee | 114 | |
jberry | 0:9b057566f9ee | 115 | |
jberry | 0:9b057566f9ee | 116 | |
jberry | 0:9b057566f9ee | 117 | |
jberry | 0:9b057566f9ee | 118 | |
jberry | 0:9b057566f9ee | 119 | |
jberry | 0:9b057566f9ee | 120 | |
jberry | 0:9b057566f9ee | 121 | |
jberry | 0:9b057566f9ee | 122 | |
jberry | 0:9b057566f9ee | 123 | |
jberry | 0:9b057566f9ee | 124 | extern BusOut leds; |
jberry | 0:9b057566f9ee | 125 | |
jberry | 0:9b057566f9ee | 126 | template<> OS_PROCESS void PING_PROC::Exec() //Output stream handling process |
jberry | 0:9b057566f9ee | 127 | { |
jberry | 0:9b057566f9ee | 128 | byte M = 0; |
jberry | 0:9b057566f9ee | 129 | rangefinder PING; |
jberry | 0:9b057566f9ee | 130 | |
jberry | 0:9b057566f9ee | 131 | |
jberry | 0:9b057566f9ee | 132 | char PIN_Return_chars[100]; //defined in accelerometer.h |
jberry | 0:9b057566f9ee | 133 | |
jberry | 0:9b057566f9ee | 134 | for(;;) |
jberry | 0:9b057566f9ee | 135 | { |
jberry | 0:9b057566f9ee | 136 | if(PING_MESSAGE.wait(80)) //length of time to wait also determines how rapidly to ticker ping |
jberry | 0:9b057566f9ee | 137 | { leds = 0x2; |
jberry | 0:9b057566f9ee | 138 | M = PING_MESSAGE; //read the message that was destined for this process |
jberry | 0:9b057566f9ee | 139 | PING_MESSAGE.reset(); |
jberry | 0:9b057566f9ee | 140 | |
jberry | 0:9b057566f9ee | 141 | if(M == 0) //then ping! command was sent |
jberry | 0:9b057566f9ee | 142 | { |
jberry | 0:9b057566f9ee | 143 | { |
jberry | 0:9b057566f9ee | 144 | //OS::TISRW ISRW; //this is a critical section as timing is involved |
jberry | 0:9b057566f9ee | 145 | //this will not work though as it disables interrupts. |
jberry | 0:9b057566f9ee | 146 | PING.ping(); |
jberry | 0:9b057566f9ee | 147 | } |
jberry | 0:9b057566f9ee | 148 | |
jberry | 0:9b057566f9ee | 149 | { |
jberry | 0:9b057566f9ee | 150 | //TCritSect cs; |
jberry | 0:9b057566f9ee | 151 | sprintf(PIN_Return_chars, "PING:%f\n", PING.get_time()); // |
jberry | 0:9b057566f9ee | 152 | } |
jberry | 0:9b057566f9ee | 153 | //Ser_out_Mutex.Lock(); |
jberry | 0:9b057566f9ee | 154 | //if(TX_Channel.get_free_size() > strlen(Return_chars)+1) |
jberry | 0:9b057566f9ee | 155 | TX_channel.write(PIN_Return_chars, strlen(PIN_Return_chars)+1); //output the ping result |
jberry | 0:9b057566f9ee | 156 | //Ser_out_Mutex.Unlock(); |
jberry | 0:9b057566f9ee | 157 | |
jberry | 0:9b057566f9ee | 158 | |
jberry | 0:9b057566f9ee | 159 | } |
jberry | 0:9b057566f9ee | 160 | } |
jberry | 0:9b057566f9ee | 161 | if(M == 1) //ticker ping |
jberry | 0:9b057566f9ee | 162 | { |
jberry | 0:9b057566f9ee | 163 | { |
jberry | 0:9b057566f9ee | 164 | //OS::TISRW ISRW; //this is a critical section as timing is involved |
jberry | 0:9b057566f9ee | 165 | PING.ping(); |
jberry | 0:9b057566f9ee | 166 | } |
jberry | 0:9b057566f9ee | 167 | //Sleep(2); |
jberry | 0:9b057566f9ee | 168 | { |
jberry | 0:9b057566f9ee | 169 | //TCritSect cs; |
jberry | 0:9b057566f9ee | 170 | sprintf(PIN_Return_chars, "PING:%f\n", PING.get_time()); //"PING:%f\n" |
jberry | 0:9b057566f9ee | 171 | } |
jberry | 0:9b057566f9ee | 172 | //Ser_out_Mutex.Lock(); |
jberry | 0:9b057566f9ee | 173 | //if(TX_Channel.get_free_size() > strlen(Return_chars)+1) |
jberry | 0:9b057566f9ee | 174 | TX_channel.write(PIN_Return_chars, strlen(PIN_Return_chars)+1); //output the result |
jberry | 0:9b057566f9ee | 175 | //Ser_out_Mutex.Unlock(); |
jberry | 0:9b057566f9ee | 176 | //TX_flag.Signal(); |
jberry | 0:9b057566f9ee | 177 | |
jberry | 0:9b057566f9ee | 178 | } |
jberry | 0:9b057566f9ee | 179 | |
jberry | 0:9b057566f9ee | 180 | //PING_MESSAGE.reset(); |
jberry | 0:9b057566f9ee | 181 | |
jberry | 0:9b057566f9ee | 182 | leds = 0x2; |
jberry | 0:9b057566f9ee | 183 | |
jberry | 0:9b057566f9ee | 184 | } |
jberry | 0:9b057566f9ee | 185 | } |
jberry | 0:9b057566f9ee | 186 | |
jberry | 0:9b057566f9ee | 187 | |
jberry | 0:9b057566f9ee | 188 | #endif |
jberry | 0:9b057566f9ee | 189 | |
jberry | 0:9b057566f9ee | 190 | //void enable_ticker() { |
jberry | 0:9b057566f9ee | 191 | // TICKER.attach_us(this, &rangefinder::ping, TX_RATE); //call ping() every 40 ms |
jberry | 0:9b057566f9ee | 192 | //send_ticker.attach_us(this, &rangefinder::send_data, TX_RATE); //send the data out every 40 ms |
jberry | 0:9b057566f9ee | 193 | //} |
jberry | 0:9b057566f9ee | 194 | //void disable_ticker() { |
jberry | 0:9b057566f9ee | 195 | //printf("ticker should now be detached.\n"); |
jberry | 0:9b057566f9ee | 196 | // TICKER.detach(); |
jberry | 0:9b057566f9ee | 197 | //send_ticker.detach(); |
jberry | 0:9b057566f9ee | 198 | //} |
jberry | 0:9b057566f9ee | 199 | |
jberry | 0:9b057566f9ee | 200 | |
jberry | 0:9b057566f9ee | 201 | |
jberry | 0:9b057566f9ee | 202 | |
jberry | 0:9b057566f9ee | 203 |