the fish that looks like a jet
Dependencies: ADXL345 ADXL345_I2C IMUfilter ITG3200 mbed Servo
motor_controller.cpp
- Committer:
- rkk
- Date:
- 2014-01-31
- Revision:
- 9:8dd7a76756e2
- Parent:
- 8:0574a5db1fc4
- Child:
- 10:d9f1037f0cb0
File content as of revision 9:8dd7a76756e2:
#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); outA=new DigitalOut(A); outB=new DigitalOut(B); outA->write(0); outB->write(1); timestamp=0; ome1 = 0.0; amp1 = 0.0; phi1 = 0.0; } PololuMController::~PololuMController() { delete pwm; delete outA; delete outB; } void PololuMController::setspeed(float speed) { pwm->write(speed); return; } void PololuMController::setpolarspeed(float speed) { if (speed>=0) { outA->write(0); outB->write(1); pwm->write(abs(speed)); } else { outA->write(1); outB->write(0); pwm->write(abs(speed)); } return; } void PololuMController::reverse() { outA->write(!(outA->read())); outB->write(!(outB->read())); return; } void PololuMController::drive_sinusoidal(float currentTime, float amplitude, float frequency) { float ome2 = 2.0* MATH_PI * frequency; float divisor = (ome2*currentTime); float dutycycle; float phi2; if(divisor > 0.0 && amplitude > 0.0) { phi2 = asin(amp1/amplitude*sin(ome1 * currentTime + phi1))/divisor; dutycycle = amplitude*sin(ome2 * currentTime + phi2); phi1 = phi2; } else{ dutycycle = 0.0; } setpolarspeed(dutycycle); //set previous values ome1 = ome2; amp1 = amplitude; 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; }