My modifications/additions to the code
Dependencies: ADXL345 ADXL345_I2C IMUfilter ITG3200 Servo fishgait mbed-rtos mbed pixy_cam
Fork of robotic_fish_ver_4_8 by
Revision 5:090ef6275773, committed 2014-01-28
- Comitter:
- sandwich
- Date:
- Tue Jan 28 23:59:21 2014 +0000
- Parent:
- 4:f081a97ca000
- Child:
- 6:a4d6f3e4bf28
- Commit message:
- added interrupts
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PwmReader.cpp Tue Jan 28 23:59:21 2014 +0000 @@ -0,0 +1,44 @@ +#include "PwmReader.h" + +//Constructors +PwmReader::PwmReader(PinName pwmInPort) +{ + di = new InterruptIn(pwmInPort); + + lastRise = 0; + period = 0; + duty = 0.0; + di->rise(this,&PwmReader::pwmRise); // attach the address of the flip function to the rising edge + di->fall(this,&PwmReader::pwmFall); +} + +PwmReader::~PwmReader() +{ + delete di; +} + +// public methods +float PwmReader::getDuty() +{ + return duty; +} + +// private methods +void PwmReader::pwmRise() +{ + int rise = t.read_us(); + if( (lastRise > 0) && (rise > lastRise) ) { + period = rise - lastRise; + } + lastRise = rise; + +} + +void PwmReader::pwmFall() +{ + int fallTime = t.read_us(); + if(period > 0 ) { + int delta = fallTime - lastRise; + duty = float(delta)/float(period); + } +} \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PwmReader.h Tue Jan 28 23:59:21 2014 +0000 @@ -0,0 +1,23 @@ +#pragma once +#include "mbed.h" + +class PwmReader +{ + public: + PwmReader(); + PwmReader(PinName pwmInPort); + ~PwmReader(); + + float getDuty(); //0 to 1 + + protected: + void pwmRise(); //0 to 1 + void pwmFall(); //-1 to 1 + + InterruptIn* di; + int lastRise; + int period; + float duty; + Timer t; + +}; \ No newline at end of file
--- a/guardian.cpp Fri Jan 24 22:04:41 2014 +0000 +++ b/guardian.cpp Tue Jan 28 23:59:21 2014 +0000 @@ -9,7 +9,7 @@ aux=new Servo(auxpin); */ gain=new Servo(gainpin); - mod->write(0.5); + mod->write(0.5); //set autopilot to off ail->write(0.5); elv->write(0.5); /*rud->write(0.5); @@ -23,13 +23,13 @@ delete ail, mod, elv, rud, aux, gain; } -void Guardian::set3D() +void Guardian::set3D() //set autopilot to 3D { mod->write(1.00); return; } -void Guardian::set2D() +void Guardian::set2D() //set autopilot 2D { mod->write(0.00); return; @@ -44,11 +44,11 @@ void Guardian::calibrate() //must be done within 15 sec of power on { set2D(); - wait(500); + Thread::wait(500); set3D(); - wait(500); + Thread::wait(500); set2D(); - wait(2000); + Thread::wait(2000); //now look for the twitch return; }
--- a/guardian.h Fri Jan 24 22:04:41 2014 +0000 +++ b/guardian.h Tue Jan 28 23:59:21 2014 +0000 @@ -1,6 +1,7 @@ #pragma once #include "mbed.h" #include "Servo.h" +#include "rtos.h" class Guardian {
--- a/main.cpp Fri Jan 24 22:04:41 2014 +0000 +++ b/main.cpp Tue Jan 28 23:59:21 2014 +0000 @@ -4,7 +4,7 @@ #include "IMU.h" #include "Servo.h" #include "rtos.h" - +#include "PwmReader.h" bool quit=false; PololuMController mcon(p22, p6, p5); @@ -13,13 +13,22 @@ Serial xbee(p13, p14); Serial pc(USBTX, USBRX); + //IMU imu(0.1, 0.3, 0.005, 0.005); void motor_thread(void const *args) { - + Timer t; t.start(); - while (quit==false) { - mcon.drive_sinusoidal(t.read(), 1, 2*3.14, 0); + PwmReader thrDutyCycl(p7); + + PwmReader freqDutyCycl(p8); + + while (quit==false) { + float td = thrDutyCycl.getDuty(); + float fd = freqDutyCycl.getDuty(); + pc.printf("throttle: %f frequency: %f\n",td,fd); + mcon.drive_sinusoidal(t.read(), 1, 2*3.141 ,0); // (time,amplitude,frequency,freqoffset) + Thread::wait(10); } t.stop(); } @@ -27,15 +36,20 @@ int main() { Thread thread(motor_thread); - ap.calibrate(); - ap.set2D(); - while(1) { - char buf[128]; - if (xbee.readable()) - { - xbee.gets(buf, 128); - pc.puts(buf); - } - memset(buf, 0, 128); + //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); + + + Thread::wait(10); } }
--- a/motor_controller.cpp Fri Jan 24 22:04:41 2014 +0000 +++ b/motor_controller.cpp Tue Jan 28 23:59:21 2014 +0000 @@ -51,4 +51,4 @@ { setpolarspeed(a*sin(w*cur_time+phi)); return; -} \ No newline at end of file +}