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
motor_controller.cpp
- Committer:
- sandwich
- Date:
- 2014-01-30
- Revision:
- 7:e005cfaff8d1
- Parent:
- 6:a4d6f3e4bf28
- Child:
- 8:0574a5db1fc4
File content as of revision 7:e005cfaff8d1:
#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; } 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 dutyCycle, float frequency) { 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; }