Program to control an accelerometer, motors and a rangefinder using the ScmRTOS ported to mbed. (Work in progress and buggy)
Hexacopter/accelerometer.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 ACCELEROMETER_H |
jberry | 0:9b057566f9ee | 3 | #define ACCELEROMETER_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 X_axis p28 |
jberry | 0:9b057566f9ee | 9 | #define Y_axis p27 |
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 | |
jberry | 0:9b057566f9ee | 14 | |
jberry | 0:9b057566f9ee | 15 | class accelerometer { |
jberry | 0:9b057566f9ee | 16 | public: |
jberry | 0:9b057566f9ee | 17 | accelerometer() |
jberry | 0:9b057566f9ee | 18 | :X_int(X_axis), |
jberry | 0:9b057566f9ee | 19 | Y_int(Y_axis) |
jberry | 0:9b057566f9ee | 20 | //X_input(X_axis), |
jberry | 0:9b057566f9ee | 21 | //Y_input(Y_axis) |
jberry | 0:9b057566f9ee | 22 | { |
jberry | 0:9b057566f9ee | 23 | //led3 = led4 = 0; |
jberry | 0:9b057566f9ee | 24 | X_time = Y_time = 0; |
jberry | 0:9b057566f9ee | 25 | //enable_flag = false; |
jberry | 0:9b057566f9ee | 26 | enable_X_rising_flag = enable_X_falling_flag = enable_Y_rising_flag = enable_Y_falling_flag = false; |
jberry | 0:9b057566f9ee | 27 | X_int.mode(PullDown); |
jberry | 0:9b057566f9ee | 28 | Y_int.mode(PullDown); |
jberry | 0:9b057566f9ee | 29 | //Y_input.mode(PullDown); |
jberry | 0:9b057566f9ee | 30 | //X_input.mode(PullDown); |
jberry | 0:9b057566f9ee | 31 | X_timer.reset(); |
jberry | 0:9b057566f9ee | 32 | Y_timer.reset(); |
jberry | 0:9b057566f9ee | 33 | set_interrupts(); |
jberry | 0:9b057566f9ee | 34 | enable_X_rising_flag = enable_Y_rising_flag = true; |
jberry | 0:9b057566f9ee | 35 | } |
jberry | 0:9b057566f9ee | 36 | |
jberry | 0:9b057566f9ee | 37 | |
jberry | 0:9b057566f9ee | 38 | void set_interrupts() |
jberry | 0:9b057566f9ee | 39 | { |
jberry | 0:9b057566f9ee | 40 | X_int.fall(this, &accelerometer::set_X_time); |
jberry | 0:9b057566f9ee | 41 | X_int.rise(this, &accelerometer::start_X_timing); |
jberry | 0:9b057566f9ee | 42 | Y_int.fall(this, &accelerometer::set_Y_time); |
jberry | 0:9b057566f9ee | 43 | Y_int.rise(this, &accelerometer::start_Y_timing); |
jberry | 0:9b057566f9ee | 44 | } |
jberry | 0:9b057566f9ee | 45 | |
jberry | 0:9b057566f9ee | 46 | void start_X_timing() |
jberry | 0:9b057566f9ee | 47 | { |
jberry | 0:9b057566f9ee | 48 | OS::TISRW ISRW; |
jberry | 0:9b057566f9ee | 49 | if(enable_X_rising_flag == true) |
jberry | 0:9b057566f9ee | 50 | { |
jberry | 0:9b057566f9ee | 51 | X_timer.start(); |
jberry | 0:9b057566f9ee | 52 | enable_X_falling_flag = true; |
jberry | 0:9b057566f9ee | 53 | //printf("In start_x_time().\n"); |
jberry | 0:9b057566f9ee | 54 | |
jberry | 0:9b057566f9ee | 55 | } |
jberry | 0:9b057566f9ee | 56 | |
jberry | 0:9b057566f9ee | 57 | |
jberry | 0:9b057566f9ee | 58 | } |
jberry | 0:9b057566f9ee | 59 | void start_Y_timing() |
jberry | 0:9b057566f9ee | 60 | { |
jberry | 0:9b057566f9ee | 61 | OS::TISRW ISRW; |
jberry | 0:9b057566f9ee | 62 | if(enable_Y_rising_flag == true) |
jberry | 0:9b057566f9ee | 63 | { |
jberry | 0:9b057566f9ee | 64 | Y_timer.start(); |
jberry | 0:9b057566f9ee | 65 | enable_Y_falling_flag = true; |
jberry | 0:9b057566f9ee | 66 | |
jberry | 0:9b057566f9ee | 67 | } |
jberry | 0:9b057566f9ee | 68 | |
jberry | 0:9b057566f9ee | 69 | |
jberry | 0:9b057566f9ee | 70 | } |
jberry | 0:9b057566f9ee | 71 | |
jberry | 0:9b057566f9ee | 72 | void set_X_time() { |
jberry | 0:9b057566f9ee | 73 | if(enable_X_falling_flag == true) |
jberry | 0:9b057566f9ee | 74 | { |
jberry | 0:9b057566f9ee | 75 | OS::TISRW ISRW; |
jberry | 0:9b057566f9ee | 76 | X_timer.stop(); |
jberry | 0:9b057566f9ee | 77 | X_time = X_timer.read_us(); |
jberry | 0:9b057566f9ee | 78 | X_timer.reset(); |
jberry | 0:9b057566f9ee | 79 | // led3 = !led3; |
jberry | 0:9b057566f9ee | 80 | //printf("In set_x_time().\n"); |
jberry | 0:9b057566f9ee | 81 | |
jberry | 0:9b057566f9ee | 82 | |
jberry | 0:9b057566f9ee | 83 | } |
jberry | 0:9b057566f9ee | 84 | } |
jberry | 0:9b057566f9ee | 85 | |
jberry | 0:9b057566f9ee | 86 | |
jberry | 0:9b057566f9ee | 87 | void set_Y_time() { |
jberry | 0:9b057566f9ee | 88 | if(enable_Y_falling_flag == true) |
jberry | 0:9b057566f9ee | 89 | { |
jberry | 0:9b057566f9ee | 90 | OS::TISRW ISRW; |
jberry | 0:9b057566f9ee | 91 | Y_timer.stop(); |
jberry | 0:9b057566f9ee | 92 | Y_time = Y_timer.read_us(); |
jberry | 0:9b057566f9ee | 93 | Y_timer.reset(); |
jberry | 0:9b057566f9ee | 94 | // led4 = !led4; |
jberry | 0:9b057566f9ee | 95 | |
jberry | 0:9b057566f9ee | 96 | |
jberry | 0:9b057566f9ee | 97 | } |
jberry | 0:9b057566f9ee | 98 | } |
jberry | 0:9b057566f9ee | 99 | |
jberry | 0:9b057566f9ee | 100 | float get_X_time() |
jberry | 0:9b057566f9ee | 101 | { |
jberry | 0:9b057566f9ee | 102 | return X_time; |
jberry | 0:9b057566f9ee | 103 | } |
jberry | 0:9b057566f9ee | 104 | |
jberry | 0:9b057566f9ee | 105 | float get_Y_time() |
jberry | 0:9b057566f9ee | 106 | { |
jberry | 0:9b057566f9ee | 107 | return Y_time; |
jberry | 0:9b057566f9ee | 108 | } |
jberry | 0:9b057566f9ee | 109 | |
jberry | 0:9b057566f9ee | 110 | |
jberry | 0:9b057566f9ee | 111 | private: |
jberry | 0:9b057566f9ee | 112 | Timer X_timer; |
jberry | 0:9b057566f9ee | 113 | Timer Y_timer; |
jberry | 0:9b057566f9ee | 114 | float X_time; //time in us |
jberry | 0:9b057566f9ee | 115 | float Y_time; |
jberry | 0:9b057566f9ee | 116 | InterruptIn X_int; |
jberry | 0:9b057566f9ee | 117 | InterruptIn Y_int; |
jberry | 0:9b057566f9ee | 118 | //DigitalIn X_input; |
jberry | 0:9b057566f9ee | 119 | //DigitalIn Y_input; |
jberry | 0:9b057566f9ee | 120 | bool enable_X_rising_flag; |
jberry | 0:9b057566f9ee | 121 | bool enable_X_falling_flag; |
jberry | 0:9b057566f9ee | 122 | bool enable_Y_rising_flag; |
jberry | 0:9b057566f9ee | 123 | bool enable_Y_falling_flag; |
jberry | 0:9b057566f9ee | 124 | }; |
jberry | 0:9b057566f9ee | 125 | |
jberry | 0:9b057566f9ee | 126 | |
jberry | 0:9b057566f9ee | 127 | |
jberry | 0:9b057566f9ee | 128 | |
jberry | 0:9b057566f9ee | 129 | |
jberry | 0:9b057566f9ee | 130 | |
jberry | 0:9b057566f9ee | 131 | |
jberry | 0:9b057566f9ee | 132 | |
jberry | 0:9b057566f9ee | 133 | extern BusOut leds; |
jberry | 0:9b057566f9ee | 134 | |
jberry | 0:9b057566f9ee | 135 | char ACC_Return_chars[100] = ""; |
jberry | 0:9b057566f9ee | 136 | |
jberry | 0:9b057566f9ee | 137 | |
jberry | 0:9b057566f9ee | 138 | //typedef OS::process<OS::pr2, 1400> Accelo_proc; |
jberry | 0:9b057566f9ee | 139 | template<> OS_PROCESS void Accelo_proc::Exec() //Output stream handling process |
jberry | 0:9b057566f9ee | 140 | { |
jberry | 0:9b057566f9ee | 141 | |
jberry | 0:9b057566f9ee | 142 | accelerometer ACC; |
jberry | 0:9b057566f9ee | 143 | |
jberry | 0:9b057566f9ee | 144 | byte M = 0; |
jberry | 0:9b057566f9ee | 145 | for(;;) |
jberry | 0:9b057566f9ee | 146 | { |
jberry | 0:9b057566f9ee | 147 | if(ACCELEROMETER_MESSAGE.wait(70)) //length of time to wait also determines how rapidly to return accelerometer data |
jberry | 0:9b057566f9ee | 148 | { leds = 0x2; |
jberry | 0:9b057566f9ee | 149 | M = ACCELEROMETER_MESSAGE; //read the message that was destined for this process |
jberry | 0:9b057566f9ee | 150 | ACCELEROMETER_MESSAGE.reset(); |
jberry | 0:9b057566f9ee | 151 | |
jberry | 0:9b057566f9ee | 152 | if(M == 0) //then return x and y data once |
jberry | 0:9b057566f9ee | 153 | { |
jberry | 0:9b057566f9ee | 154 | sprintf(ACC_Return_chars, "ACC_X:%f\nACC_Y:%f\n", ACC.get_X_time(), ACC.get_Y_time()); // ready return string |
jberry | 0:9b057566f9ee | 155 | //if(!Ser_out_Mutex.IsLocked()) |
jberry | 0:9b057566f9ee | 156 | { |
jberry | 0:9b057566f9ee | 157 | //Ser_out_Mutex.Lock(); |
jberry | 0:9b057566f9ee | 158 | //if(TX_Channel.get_free_size() > strlen(Return_chars)+1) |
jberry | 0:9b057566f9ee | 159 | TX_channel.write(ACC_Return_chars, strlen(ACC_Return_chars)+1); //output the data! |
jberry | 0:9b057566f9ee | 160 | //Ser_out_Mutex.Unlock(); |
jberry | 0:9b057566f9ee | 161 | } |
jberry | 0:9b057566f9ee | 162 | |
jberry | 0:9b057566f9ee | 163 | |
jberry | 0:9b057566f9ee | 164 | |
jberry | 0:9b057566f9ee | 165 | } |
jberry | 0:9b057566f9ee | 166 | } |
jberry | 0:9b057566f9ee | 167 | if(M == 1) //ticker |
jberry | 0:9b057566f9ee | 168 | { |
jberry | 0:9b057566f9ee | 169 | |
jberry | 0:9b057566f9ee | 170 | { |
jberry | 0:9b057566f9ee | 171 | //TCritSect cs; |
jberry | 0:9b057566f9ee | 172 | sprintf(ACC_Return_chars, "ACC_X:%f\nACC_Y:%f\n", ACC.get_X_time(), ACC.get_Y_time()); // ready return string //"ACC_X:%f\nACC_Y:%f\n" |
jberry | 0:9b057566f9ee | 173 | } |
jberry | 0:9b057566f9ee | 174 | //if(!Ser_out_Mutex.IsLocked()) |
jberry | 0:9b057566f9ee | 175 | { |
jberry | 0:9b057566f9ee | 176 | //Ser_out_Mutex.Lock(); |
jberry | 0:9b057566f9ee | 177 | //if(TX_Channel.get_free_size() > strlen(Return_chars)+1) |
jberry | 0:9b057566f9ee | 178 | TX_channel.write(ACC_Return_chars, strlen(ACC_Return_chars)+1); //output the data! |
jberry | 0:9b057566f9ee | 179 | //Ser_out_Mutex.Unlock(); |
jberry | 0:9b057566f9ee | 180 | } |
jberry | 0:9b057566f9ee | 181 | //TX_flag.Signal(); |
jberry | 0:9b057566f9ee | 182 | |
jberry | 0:9b057566f9ee | 183 | } |
jberry | 0:9b057566f9ee | 184 | |
jberry | 0:9b057566f9ee | 185 | leds = 0x2; |
jberry | 0:9b057566f9ee | 186 | |
jberry | 0:9b057566f9ee | 187 | |
jberry | 0:9b057566f9ee | 188 | } |
jberry | 0:9b057566f9ee | 189 | } |
jberry | 0:9b057566f9ee | 190 | |
jberry | 0:9b057566f9ee | 191 | |
jberry | 0:9b057566f9ee | 192 | #endif |
jberry | 0:9b057566f9ee | 193 | |
jberry | 0:9b057566f9ee | 194 | |
jberry | 0:9b057566f9ee | 195 | |
jberry | 0:9b057566f9ee | 196 | |
jberry | 0:9b057566f9ee | 197 |