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 17:6970aef8154b, committed 2014-05-22
- Comitter:
- rkk
- Date:
- Thu May 22 04:32:19 2014 +0000
- Parent:
- 16:79cfe6201318
- Child:
- 18:9ba4566f2361
- Commit message:
- jet fish with simple yaw motion and added channels for pwm in and pwm out
Changed in this revision
--- a/MainController.cpp Mon Feb 17 14:04:09 2014 +0000 +++ b/MainController.cpp Thu May 22 04:32:19 2014 +0000 @@ -1,20 +1,28 @@ #include "MainController.h" MainController::MainController() - :ch3(p16,0.054,0.092), // frequency + :ch1(p18,0.054,0.092), // pitch + ch2(p30,0.054,0.092), // roll + ch3(p16,0.054,0.092), // frequency ch4(p17,0.055,0.092), //rudder ch6(p15,0.055,0.092), //volume - mcon(p22, p6, p7), // change p5 to p7, because p5 is burned through - ap(p25, p26) + mcon(p22, p6, p7), // change pin p5 to p7, because p5 is burned through + ap(p25, p26), + leftservo(p21), + rightservo(p22) + { wait_ms(50); vol = 0.0; - frq = 10.0; + frq = 1.0; rud = 0.5; + pitch = 0.5; frqMin = 0.4; // hardcoded frqMax = 2.5; //hardcoded - goofftime = 0.0; - switched = false; + change = 0; + state = 0; + //goofftime = 0.0; + //switched = false; } void MainController::control() @@ -25,18 +33,46 @@ //read new inputs vol = this->calculateVolume(); frq = this->calculateFrequency(); - rud = this->calculateRudder(); //not used right now + rud = this->calculateRudder(); timer1.reset(); curTime = 0.0; - + // rudder value used to define the trapezoid shape - amplitude = vol * ( 2* rud + 1.0); - //amplitude = vol * 3.0; + // amplitude = vol * ( 2* rud + 1.0); // varied from 1 to 5 + amplitude = vol * 4.0; + + //reset change to zero + change = 0; - switched = false; - goofftime = (vol/(2*frq)); - + if(state == 1) { + if(rud < 0.75) { + change = -1; + state = 0; + } + } + else if (state == -1) { + if(rud> 0.25) { + change = 1; + state = 0; + } + } + else { + if(rud < 0.25) { + change = -1; + state = -1; + } else if(rud > 0.75) { + change = 1; + state = 1; + } + } + //switched = false; + //goofftime = (vol/(2*frq)); } + + //Set Servo Values + pitch = this->calculatePitch(); + leftservo = pitch; + rightservo = 1.0 - pitch; // if (curTime > 1/(2*frq) && (switched == false)) // { @@ -55,12 +91,18 @@ // amplitude = 0.0; // } - dutyCycle = saturate(amplitude * sin( 2.0 * MATH_PI * frq * curTime )); - + // saturate ensures the duty cycle does not exceed 1, + + if((change == 1) && (curTime > 1/(2*frq))) { + dutyCycle = 0.0; + } else if((change == -1) && (curTime < 1/(2*frq))) { + dutyCycle = 0.0; + } else { + dutyCycle = saturate(amplitude * sin( 2.0 * MATH_PI * frq * curTime )); + } mcon.setpolarspeed(dutyCycle); } - float MainController::calculateFrequency() { return ((frqMax - frqMin) * ch3.dutycyclescaledup() + frqMin); @@ -76,9 +118,15 @@ return (ch4.dutycyclescaledup()); } +float MainController::calculatePitch() +{ + return (ch1.dutycyclescaledup()); +} + void MainController::start() { timer1.start(); + ticker1.attach(this, &MainController::control,0.001); //Autopilot guardian //ap.calibrate(); @@ -120,6 +168,11 @@ return rud; } +float MainController::getPitch() +{ + return pitch; +} + //signum function float MainController::signum(float input) {
--- a/MainController.h Mon Feb 17 14:04:09 2014 +0000 +++ b/MainController.h Thu May 22 04:32:19 2014 +0000 @@ -6,6 +6,8 @@ #include "motor_controller.h" #include "guardian.h" //#include "IMU.h" +#include "Servo.h" + #define MATH_PI 3.14159265359 @@ -33,6 +35,7 @@ float getVolume(); float getAmplitude(); float getRudder(); + float getPitch(); /** Stop the main controller * @@ -46,16 +49,20 @@ float calculateFrequency(); float calculateVolume(); float calculateRudder(); + float calculatePitch(); float signum(float input); float saturate(float input); private: + PwmIn ch1; + PwmIn ch2; PwmIn ch3; PwmIn ch4; PwmIn ch6; PololuMController mcon; Guardian ap; - + Servo leftservo; + Servo rightservo; Timer timer1; Ticker ticker1; @@ -67,8 +74,12 @@ float frqMax; float amplitude; float rud; - float goofftime; - bool switched; + float pitch; + + int state; + int change; + //float goofftime; + //bool switched; }; #endif
--- a/main.cpp Mon Feb 17 14:04:09 2014 +0000 +++ b/main.cpp Thu May 22 04:32:19 2014 +0000 @@ -18,8 +18,8 @@ while(true) { - pc.printf("frq: %f, vol: %f, amp: %f, rud: %f, dut: %f, t: %f\n", - mainCtrl.getFrequency(), mainCtrl.getVolume(), mainCtrl.getAmplitude(), mainCtrl.getRudder(), mainCtrl.getDutyCycle(),t.read()); + pc.printf("frq: %f, vol: %f, amp: %f, rud: %f, dut: %f, pit: %f, t: %f\n", + mainCtrl.getFrequency(), mainCtrl.getVolume(), mainCtrl.getAmplitude(), mainCtrl.getRudder(), mainCtrl.getDutyCycle(), mainCtrl.getPitch(), t.read()); wait_ms(100); }