jetfishteam / Mbed 2 deprecated robotic_fish_ver_4_9_pixy

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 jetfishteam

motor_controller.cpp@7:e005cfaff8d1, 2014-01-30 (annotated)

Committer:
sandwich
Date:
Thu Jan 30 02:04:23 2014 +0000
Revision:
7:e005cfaff8d1
Parent:
6:a4d6f3e4bf28
Child:
8:0574a5db1fc4
removed rtos and added interrupt reading

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sandwich 0:ff9bc5f69c57 1 #include "motor_controller.h"
sandwich 0:ff9bc5f69c57 2
sandwich 7:e005cfaff8d1 3 float sigm(float input)
sandwich 7:e005cfaff8d1 4 {
sandwich 7:e005cfaff8d1 5 if (input>0)
sandwich 7:e005cfaff8d1 6 return 1;
sandwich 7:e005cfaff8d1 7 else if (input<0)
sandwich 7:e005cfaff8d1 8 return -1;
sandwich 7:e005cfaff8d1 9 else
sandwich 7:e005cfaff8d1 10 return 0;
sandwich 7:e005cfaff8d1 11 }
sandwich 7:e005cfaff8d1 12
sandwich 0:ff9bc5f69c57 13 PololuMController::PololuMController(PinName pwmport, PinName A, PinName B)
sandwich 0:ff9bc5f69c57 14 {
sandwich 0:ff9bc5f69c57 15 pwm=new PwmOut(pwmport);
sandwich 0:ff9bc5f69c57 16 outA=new DigitalOut(A);
sandwich 0:ff9bc5f69c57 17 outB=new DigitalOut(B);
sandwich 0:ff9bc5f69c57 18 outA->write(0);
sandwich 0:ff9bc5f69c57 19 outB->write(1);
sandwich 0:ff9bc5f69c57 20 timestamp=0;
sandwich 0:ff9bc5f69c57 21 }
sandwich 0:ff9bc5f69c57 22
sandwich 0:ff9bc5f69c57 23 PololuMController::~PololuMController()
sandwich 0:ff9bc5f69c57 24 {
sandwich 0:ff9bc5f69c57 25 delete pwm;
sandwich 0:ff9bc5f69c57 26 delete outA;
sandwich 0:ff9bc5f69c57 27 delete outB;
sandwich 0:ff9bc5f69c57 28 }
sandwich 0:ff9bc5f69c57 29
sandwich 0:ff9bc5f69c57 30 void PololuMController::setspeed(float speed)
sandwich 0:ff9bc5f69c57 31 {
sandwich 0:ff9bc5f69c57 32 pwm->write(speed);
sandwich 0:ff9bc5f69c57 33 return;
sandwich 0:ff9bc5f69c57 34 }
sandwich 0:ff9bc5f69c57 35
sandwich 0:ff9bc5f69c57 36 void PololuMController::setpolarspeed(float speed)
sandwich 0:ff9bc5f69c57 37 {
sandwich 0:ff9bc5f69c57 38 if (speed>=0)
sandwich 0:ff9bc5f69c57 39 {
sandwich 0:ff9bc5f69c57 40 outA->write(0);
sandwich 0:ff9bc5f69c57 41 outB->write(1);
sandwich 0:ff9bc5f69c57 42 pwm->write(abs(speed));
sandwich 0:ff9bc5f69c57 43 }
sandwich 0:ff9bc5f69c57 44 else
sandwich 0:ff9bc5f69c57 45 {
sandwich 0:ff9bc5f69c57 46 outA->write(1);
sandwich 0:ff9bc5f69c57 47 outB->write(0);
sandwich 0:ff9bc5f69c57 48 pwm->write(abs(speed));
sandwich 0:ff9bc5f69c57 49 }
sandwich 0:ff9bc5f69c57 50 return;
sandwich 0:ff9bc5f69c57 51 }
sandwich 0:ff9bc5f69c57 52
sandwich 0:ff9bc5f69c57 53 void PololuMController::reverse()
sandwich 0:ff9bc5f69c57 54 {
sandwich 0:ff9bc5f69c57 55 outA->write(!(outA->read()));
sandwich 0:ff9bc5f69c57 56 outB->write(!(outB->read()));
sandwich 0:ff9bc5f69c57 57 return;
sandwich 0:ff9bc5f69c57 58 }
sandwich 0:ff9bc5f69c57 59
rkk 6:a4d6f3e4bf28 60 void PololuMController::drive_sinusoidal(float currentTime, float dutyCycle, float frequency)
sandwich 0:ff9bc5f69c57 61 {
sandwich 7:e005cfaff8d1 62
sandwich 7:e005cfaff8d1 63 setpolarspeed(dutyCycle*sin( 2.0* MATH_PI* frequency * currentTime));
sandwich 0:ff9bc5f69c57 64 return;
sandwich 5:090ef6275773 65 }
sandwich 7:e005cfaff8d1 66
sandwich 7:e005cfaff8d1 67 void PololuMController::drive_rectangular(float currentTime, float amplitude, float frequency)
sandwich 7:e005cfaff8d1 68 {
sandwich 7:e005cfaff8d1 69 //setpolarspeed(dutyCycle*sin( 2.0* MATH_PI* f * currentTime));
sandwich 7:e005cfaff8d1 70 float sinRes = sin( 2.0* MATH_PI* frequency * currentTime);
sandwich 7:e005cfaff8d1 71
sandwich 7:e005cfaff8d1 72 float dutycycle = amplitude * sigm( sinRes);
sandwich 7:e005cfaff8d1 73 setpolarspeed(dutycycle);
sandwich 7:e005cfaff8d1 74 return;
sandwich 7:e005cfaff8d1 75 }