the fish that looks like a jet
Dependencies: ADXL345 ADXL345_I2C IMUfilter ITG3200 mbed Servo
main.cpp@6:a4d6f3e4bf28, 2014-01-29 (annotated)
- Committer:
- rkk
- Date:
- Wed Jan 29 05:04:50 2014 +0000
- Revision:
- 6:a4d6f3e4bf28
- Parent:
- 5:090ef6275773
- Child:
- 7:e005cfaff8d1
added pam ira
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
sandwich | 0:ff9bc5f69c57 | 1 | #include "mbed.h" |
sandwich | 0:ff9bc5f69c57 | 2 | #include "motor_controller.h" |
sandwich | 2:430c068cf570 | 3 | #include "guardian.h" |
sandwich | 0:ff9bc5f69c57 | 4 | #include "IMU.h" |
sandwich | 2:430c068cf570 | 5 | #include "Servo.h" |
sandwich | 2:430c068cf570 | 6 | #include "rtos.h" |
sandwich | 5:090ef6275773 | 7 | #include "PwmReader.h" |
rkk | 6:a4d6f3e4bf28 | 8 | |
rkk | 6:a4d6f3e4bf28 | 9 | #define THROTTLE_MAX 0.4 |
rkk | 6:a4d6f3e4bf28 | 10 | #define THROTTLE_MIN 0.2 |
rkk | 6:a4d6f3e4bf28 | 11 | #define FREQUENCY_MAX 0.4 |
rkk | 6:a4d6f3e4bf28 | 12 | #define FREQUENCY_MIN 0.2 |
rkk | 6:a4d6f3e4bf28 | 13 | #define RUDDER_MAX 0.4 |
rkk | 6:a4d6f3e4bf28 | 14 | #define RUDDER_MIN 0.2 |
rkk | 6:a4d6f3e4bf28 | 15 | |
sandwich | 2:430c068cf570 | 16 | bool quit=false; |
sandwich | 0:ff9bc5f69c57 | 17 | |
sandwich | 0:ff9bc5f69c57 | 18 | PololuMController mcon(p22, p6, p5); |
sandwich | 2:430c068cf570 | 19 | Servo servo(p21); |
sandwich | 2:430c068cf570 | 20 | Guardian ap(p21, p23, p24, p25, p26, p26); |
sandwich | 2:430c068cf570 | 21 | Serial xbee(p13, p14); |
sandwich | 2:430c068cf570 | 22 | Serial pc(USBTX, USBRX); |
sandwich | 0:ff9bc5f69c57 | 23 | |
rkk | 6:a4d6f3e4bf28 | 24 | float throttle, frequency, rudder; |
sandwich | 5:090ef6275773 | 25 | |
sandwich | 2:430c068cf570 | 26 | //IMU imu(0.1, 0.3, 0.005, 0.005); |
sandwich | 3:666c1bae1a34 | 27 | void motor_thread(void const *args) { |
sandwich | 5:090ef6275773 | 28 | |
sandwich | 0:ff9bc5f69c57 | 29 | Timer t; |
sandwich | 0:ff9bc5f69c57 | 30 | t.start(); |
rkk | 6:a4d6f3e4bf28 | 31 | PwmReader thrDutyCycl(p7, THROTTLE_MIN, THROTTLE_MAX); |
rkk | 6:a4d6f3e4bf28 | 32 | PwmReader freqDutyCycl(p8, FREQUENCY_MIN, FREQUENCY_MAX); |
rkk | 6:a4d6f3e4bf28 | 33 | PwmReader rudDutyCycl(p9, RUDDER_MIN, RUDDER_MAX); |
sandwich | 5:090ef6275773 | 34 | |
sandwich | 5:090ef6275773 | 35 | while (quit==false) { |
rkk | 6:a4d6f3e4bf28 | 36 | throttle = thrDutyCycl.getDuty(); |
rkk | 6:a4d6f3e4bf28 | 37 | frequency = freqDutyCycl.getDuty(); |
rkk | 6:a4d6f3e4bf28 | 38 | rudder = rudDutyCycl.getDuty(); |
rkk | 6:a4d6f3e4bf28 | 39 | |
rkk | 6:a4d6f3e4bf28 | 40 | |
rkk | 6:a4d6f3e4bf28 | 41 | mcon.drive_sinusoidal(t.read(), throttle, frequency); // (time,amplitude,frequency,freqoffset) |
rkk | 6:a4d6f3e4bf28 | 42 | Thread::wait(5); |
sandwich | 0:ff9bc5f69c57 | 43 | } |
sandwich | 0:ff9bc5f69c57 | 44 | t.stop(); |
sandwich | 0:ff9bc5f69c57 | 45 | } |
sandwich | 2:430c068cf570 | 46 | |
sandwich | 2:430c068cf570 | 47 | int main() |
sandwich | 2:430c068cf570 | 48 | { |
sandwich | 2:430c068cf570 | 49 | Thread thread(motor_thread); |
sandwich | 5:090ef6275773 | 50 | //ap.calibrate(); |
sandwich | 5:090ef6275773 | 51 | //ap.set2D(); |
sandwich | 5:090ef6275773 | 52 | ap.setoff(); |
sandwich | 5:090ef6275773 | 53 | while(1) { |
sandwich | 5:090ef6275773 | 54 | //char buf[128]; |
sandwich | 5:090ef6275773 | 55 | // if (xbee.readable()) |
sandwich | 5:090ef6275773 | 56 | // { |
sandwich | 5:090ef6275773 | 57 | // xbee.gets(buf, 128); |
sandwich | 5:090ef6275773 | 58 | // //xbee.scanf("%d"); |
sandwich | 5:090ef6275773 | 59 | // pc.puts(buf); |
sandwich | 5:090ef6275773 | 60 | // } |
sandwich | 5:090ef6275773 | 61 | // memset(buf, 0, 128); |
rkk | 6:a4d6f3e4bf28 | 62 | |
rkk | 6:a4d6f3e4bf28 | 63 | //print throttle and frequency to the terminal |
rkk | 6:a4d6f3e4bf28 | 64 | pc.printf("throttle: %f frequency: %f rudder: %f\n\r",throttle,frequency,rudder); |
sandwich | 5:090ef6275773 | 65 | |
rkk | 6:a4d6f3e4bf28 | 66 | Thread::wait(500); |
sandwich | 2:430c068cf570 | 67 | } |
sandwich | 2:430c068cf570 | 68 | } |