jetfishteam / Mbed 2 deprecated jetfish

the fish that looks like a jet

Dependencies:   ADXL345 ADXL345_I2C IMUfilter ITG3200 mbed Servo

motor_controller.cpp@8:0574a5db1fc4, 2014-01-31 (annotated)

Committer:
rkk
Date:
Fri Jan 31 04:33:44 2014 +0000
Revision:
8:0574a5db1fc4
Parent:
7:e005cfaff8d1
Child:
9:8dd7a76756e2
updated the motor controller and the pwmin method

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;
rkk 8:0574a5db1fc4 21 ome1 = 0.0;
rkk 8:0574a5db1fc4 22 amp1 = 0.0;
rkk 8:0574a5db1fc4 23 phi1 = 0.0;
sandwich 0:ff9bc5f69c57 24 }
sandwich 0:ff9bc5f69c57 25
sandwich 0:ff9bc5f69c57 26 PololuMController::~PololuMController()
sandwich 0:ff9bc5f69c57 27 {
sandwich 0:ff9bc5f69c57 28 delete pwm;
sandwich 0:ff9bc5f69c57 29 delete outA;
sandwich 0:ff9bc5f69c57 30 delete outB;
sandwich 0:ff9bc5f69c57 31 }
sandwich 0:ff9bc5f69c57 32
sandwich 0:ff9bc5f69c57 33 void PololuMController::setspeed(float speed)
sandwich 0:ff9bc5f69c57 34 {
sandwich 0:ff9bc5f69c57 35 pwm->write(speed);
sandwich 0:ff9bc5f69c57 36 return;
sandwich 0:ff9bc5f69c57 37 }
sandwich 0:ff9bc5f69c57 38
sandwich 0:ff9bc5f69c57 39 void PololuMController::setpolarspeed(float speed)
sandwich 0:ff9bc5f69c57 40 {
sandwich 0:ff9bc5f69c57 41 if (speed>=0)
sandwich 0:ff9bc5f69c57 42 {
sandwich 0:ff9bc5f69c57 43 outA->write(0);
sandwich 0:ff9bc5f69c57 44 outB->write(1);
sandwich 0:ff9bc5f69c57 45 pwm->write(abs(speed));
sandwich 0:ff9bc5f69c57 46 }
sandwich 0:ff9bc5f69c57 47 else
sandwich 0:ff9bc5f69c57 48 {
sandwich 0:ff9bc5f69c57 49 outA->write(1);
sandwich 0:ff9bc5f69c57 50 outB->write(0);
sandwich 0:ff9bc5f69c57 51 pwm->write(abs(speed));
sandwich 0:ff9bc5f69c57 52 }
sandwich 0:ff9bc5f69c57 53 return;
sandwich 0:ff9bc5f69c57 54 }
sandwich 0:ff9bc5f69c57 55
sandwich 0:ff9bc5f69c57 56 void PololuMController::reverse()
sandwich 0:ff9bc5f69c57 57 {
sandwich 0:ff9bc5f69c57 58 outA->write(!(outA->read()));
sandwich 0:ff9bc5f69c57 59 outB->write(!(outB->read()));
sandwich 0:ff9bc5f69c57 60 return;
sandwich 0:ff9bc5f69c57 61 }
sandwich 0:ff9bc5f69c57 62
rkk 8:0574a5db1fc4 63 void PololuMController::drive_sinusoidal(float currentTime, float amplitude, float frequency)
sandwich 0:ff9bc5f69c57 64 {
rkk 8:0574a5db1fc4 65 float ome2 = 2.0* MATH_PI * frequency;
rkk 8:0574a5db1fc4 66 float divisor = (ome2*currentTime);
rkk 8:0574a5db1fc4 67 float phi2 = asin(amp1/amplitude*sin(ome1 * currentTime + phi1))/divisor;
sandwich 7:e005cfaff8d1 68
rkk 8:0574a5db1fc4 69 setpolarspeed(amplitude*sin(ome2 * currentTime + phi2));
rkk 8:0574a5db1fc4 70 return;
sandwich 5:090ef6275773 71 }
sandwich 7:e005cfaff8d1 72
sandwich 7:e005cfaff8d1 73 void PololuMController::drive_rectangular(float currentTime, float amplitude, float frequency)
sandwich 7:e005cfaff8d1 74 {
sandwich 7:e005cfaff8d1 75 //setpolarspeed(dutyCycle*sin( 2.0* MATH_PI* f * currentTime));
sandwich 7:e005cfaff8d1 76 float sinRes = sin( 2.0* MATH_PI* frequency * currentTime);
sandwich 7:e005cfaff8d1 77
sandwich 7:e005cfaff8d1 78 float dutycycle = amplitude * sigm( sinRes);
sandwich 7:e005cfaff8d1 79 setpolarspeed(dutycycle);
sandwich 7:e005cfaff8d1 80 return;
sandwich 7:e005cfaff8d1 81 }