the fish that looks like a jet
Dependencies: ADXL345 ADXL345_I2C IMUfilter ITG3200 mbed Servo
Revision 7:e005cfaff8d1, committed 2014-01-30
- Comitter:
- sandwich
- Date:
- Thu Jan 30 02:04:23 2014 +0000
- Parent:
- 6:a4d6f3e4bf28
- Child:
- 8:0574a5db1fc4
- Commit message:
- removed rtos and added interrupt reading
Changed in this revision
--- a/PwmReader.cpp Wed Jan 29 05:04:50 2014 +0000 +++ b/PwmReader.cpp Thu Jan 30 02:04:23 2014 +0000 @@ -14,13 +14,17 @@ lastRise = 0; period = 0; duty = 0.0; + di->mode(PullDown); di->rise(this,&PwmReader::pwmRise); // attach the address of the flip function to the rising edge di->fall(this,&PwmReader::pwmFall); + + t.start(); } PwmReader::~PwmReader() { delete di; + t.stop(); } // public methods
--- a/PwmReader.h Wed Jan 29 05:04:50 2014 +0000 +++ b/PwmReader.h Thu Jan 30 02:04:23 2014 +0000 @@ -11,8 +11,8 @@ float getDuty(); //0 to 1 protected: - void pwmRise(); //0 to 1 - void pwmFall(); //-1 to 1 + void pwmRise(); + void pwmFall(); private: InterruptIn* di;
--- a/guardian.cpp Wed Jan 29 05:04:50 2014 +0000 +++ b/guardian.cpp Thu Jan 30 02:04:23 2014 +0000 @@ -44,11 +44,11 @@ void Guardian::calibrate() //must be done within 15 sec of power on { set2D(); - Thread::wait(500); + wait(500); set3D(); - Thread::wait(500); + wait(500); set2D(); - Thread::wait(2000); + wait(2000); //now look for the twitch return; }
--- a/guardian.h Wed Jan 29 05:04:50 2014 +0000 +++ b/guardian.h Thu Jan 30 02:04:23 2014 +0000 @@ -1,7 +1,6 @@ #pragma once #include "mbed.h" #include "Servo.h" -#include "rtos.h" class Guardian {
--- a/main.cpp Wed Jan 29 05:04:50 2014 +0000 +++ b/main.cpp Thu Jan 30 02:04:23 2014 +0000 @@ -1,9 +1,9 @@ #include "mbed.h" #include "motor_controller.h" #include "guardian.h" -#include "IMU.h" +//#include "IMU.h" #include "Servo.h" -#include "rtos.h" +//#include "rtos.h" #include "PwmReader.h" #define THROTTLE_MAX 0.4 @@ -15,54 +15,34 @@ bool quit=false; +InterruptIn event(p16); PololuMController mcon(p22, p6, p5); Servo servo(p21); Guardian ap(p21, p23, p24, p25, p26, p26); Serial xbee(p13, p14); Serial pc(USBTX, USBRX); +PwmReader ch3(p16, 0.054, 0.094); +PwmReader ch6(p15, 0.054, 0.094); +Timer t; float throttle, frequency, rudder; - -//IMU imu(0.1, 0.3, 0.005, 0.005); -void motor_thread(void const *args) { - - Timer t; +int main() +{ + //ap.calibrate(); + //ap.set2D(); + ap.setoff(); t.start(); - PwmReader thrDutyCycl(p7, THROTTLE_MIN, THROTTLE_MAX); - PwmReader freqDutyCycl(p8, FREQUENCY_MIN, FREQUENCY_MAX); - PwmReader rudDutyCycl(p9, RUDDER_MIN, RUDDER_MAX); - - while (quit==false) { - throttle = thrDutyCycl.getDuty(); - frequency = freqDutyCycl.getDuty(); - rudder = rudDutyCycl.getDuty(); + while(1) { + + float vol_norm=ch3.getDuty(); + float freq_norm=ch6.getDuty(); - - mcon.drive_sinusoidal(t.read(), throttle, frequency); // (time,amplitude,frequency,freqoffset) - Thread::wait(5); + pc.printf("channel 3: %f, channel 6: %f\n", vol_norm, freq_norm); + //mcon.drive_rectangular(t.read(), vol_norm*freq_norm, 3*freq_norm); + mcon.drive_sinusoidal(t.read(), vol_norm*freq_norm, 3*freq_norm); + //pc.printf("time: %f\n\r", t.read()); + //pc.printf("throttle: %f frequency: %f rudder: %f\n\r",throttle,frequency,rudder); + //wait_ms(10); } t.stop(); } - -int main() -{ - Thread thread(motor_thread); - //ap.calibrate(); - //ap.set2D(); - ap.setoff(); - while(1) { - //char buf[128]; -// if (xbee.readable()) -// { -// xbee.gets(buf, 128); -// //xbee.scanf("%d"); -// pc.puts(buf); -// } -// memset(buf, 0, 128); - - //print throttle and frequency to the terminal - pc.printf("throttle: %f frequency: %f rudder: %f\n\r",throttle,frequency,rudder); - - Thread::wait(500); - } -}
--- a/mbed-rtos.lib Wed Jan 29 05:04:50 2014 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -http://mbed.org/users/mbed_official/code/mbed-rtos/#f88660a9bed1
--- a/motor_controller.cpp Wed Jan 29 05:04:50 2014 +0000 +++ b/motor_controller.cpp Thu Jan 30 02:04:23 2014 +0000 @@ -1,5 +1,15 @@ #include "motor_controller.h" +float sigm(float input) +{ + if (input>0) + return 1; + else if (input<0) + return -1; + else + return 0; +} + PololuMController::PololuMController(PinName pwmport, PinName A, PinName B) { pwm=new PwmOut(pwmport); @@ -49,9 +59,17 @@ void PololuMController::drive_sinusoidal(float currentTime, float dutyCycle, float frequency) { - //convert frequency form 0.0 to 1.0 - float f = (FREQ_MAX - FREQ_MIN) * f + FREQ_MIN; - - setpolarspeed(dutyCycle*sin( 2.0* MATH_PI* f * currentTime)); + + setpolarspeed(dutyCycle*sin( 2.0* MATH_PI* frequency * currentTime)); return; } + +void PololuMController::drive_rectangular(float currentTime, float amplitude, float frequency) +{ + //setpolarspeed(dutyCycle*sin( 2.0* MATH_PI* f * currentTime)); + float sinRes = sin( 2.0* MATH_PI* frequency * currentTime); + + float dutycycle = amplitude * sigm( sinRes); + setpolarspeed(dutycycle); + return; +}
--- a/motor_controller.h Wed Jan 29 05:04:50 2014 +0000 +++ b/motor_controller.h Thu Jan 30 02:04:23 2014 +0000 @@ -2,7 +2,9 @@ #include "mbed.h" #define MATH_PI 3.14159265359 #define FREQ_MIN 0.5 //Hz -#define FREQ_MAX 8 //Hz +#define FREQ_MAX 3 //Hz + +float sigm(float input); class PololuMController { @@ -19,4 +21,5 @@ void setpolarspeed(float speed); //-1 to 1 void reverse(); //only works on non-polar speed void drive_sinusoidal(float currentTime, float dutyCycle, float frequency); + void drive_rectangular(float currentTime, float amplitude, float frequency); }; \ No newline at end of file