jetfishteam / Mbed 2 deprecated jetfish

the fish that looks like a jet

Dependencies:   ADXL345 ADXL345_I2C IMUfilter ITG3200 mbed Servo

motor_controller.cpp@10:d9f1037f0cb0, 2014-01-31 (annotated)

Committer:
rkk
Date:
Fri Jan 31 05:18:19 2014 +0000
Revision:
10:d9f1037f0cb0
Parent:
9:8dd7a76756e2
Child:
11:8ec915eb70f6
still working on driving mode

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;
rkk 10:d9f1037f0cb0 24 firstTime = true;
sandwich 0:ff9bc5f69c57 25 }
sandwich 0:ff9bc5f69c57 26
sandwich 0:ff9bc5f69c57 27 PololuMController::~PololuMController()
sandwich 0:ff9bc5f69c57 28 {
sandwich 0:ff9bc5f69c57 29 delete pwm;
sandwich 0:ff9bc5f69c57 30 delete outA;
sandwich 0:ff9bc5f69c57 31 delete outB;
sandwich 0:ff9bc5f69c57 32 }
sandwich 0:ff9bc5f69c57 33
sandwich 0:ff9bc5f69c57 34 void PololuMController::setspeed(float speed)
sandwich 0:ff9bc5f69c57 35 {
sandwich 0:ff9bc5f69c57 36 pwm->write(speed);
sandwich 0:ff9bc5f69c57 37 return;
sandwich 0:ff9bc5f69c57 38 }
sandwich 0:ff9bc5f69c57 39
sandwich 0:ff9bc5f69c57 40 void PololuMController::setpolarspeed(float speed)
sandwich 0:ff9bc5f69c57 41 {
sandwich 0:ff9bc5f69c57 42 if (speed>=0)
sandwich 0:ff9bc5f69c57 43 {
sandwich 0:ff9bc5f69c57 44 outA->write(0);
sandwich 0:ff9bc5f69c57 45 outB->write(1);
sandwich 0:ff9bc5f69c57 46 pwm->write(abs(speed));
sandwich 0:ff9bc5f69c57 47 }
sandwich 0:ff9bc5f69c57 48 else
sandwich 0:ff9bc5f69c57 49 {
sandwich 0:ff9bc5f69c57 50 outA->write(1);
sandwich 0:ff9bc5f69c57 51 outB->write(0);
sandwich 0:ff9bc5f69c57 52 pwm->write(abs(speed));
sandwich 0:ff9bc5f69c57 53 }
sandwich 0:ff9bc5f69c57 54 return;
sandwich 0:ff9bc5f69c57 55 }
sandwich 0:ff9bc5f69c57 56
sandwich 0:ff9bc5f69c57 57 void PololuMController::reverse()
sandwich 0:ff9bc5f69c57 58 {
sandwich 0:ff9bc5f69c57 59 outA->write(!(outA->read()));
sandwich 0:ff9bc5f69c57 60 outB->write(!(outB->read()));
sandwich 0:ff9bc5f69c57 61 return;
sandwich 0:ff9bc5f69c57 62 }
sandwich 0:ff9bc5f69c57 63
rkk 10:d9f1037f0cb0 64 float PololuMController::drive_sinusoidal(float currentTime, float amplitude, float frequency)
sandwich 0:ff9bc5f69c57 65 {
rkk 10:d9f1037f0cb0 66
rkk 10:d9f1037f0cb0 67 //convert inputs
rkk 10:d9f1037f0cb0 68 float amp2;
rkk 10:d9f1037f0cb0 69 if(amplitude > 1.0) {
rkk 10:d9f1037f0cb0 70 amp2 = 1.0;
rkk 10:d9f1037f0cb0 71 } else if(amplitude < 0.0) {
rkk 10:d9f1037f0cb0 72 amp2 = 0.0;
rkk 10:d9f1037f0cb0 73 } else {
rkk 10:d9f1037f0cb0 74 amp2 = amplitude;
rkk 10:d9f1037f0cb0 75 }
rkk 10:d9f1037f0cb0 76 float ome2 = 2.0* MATH_PI * frequency;
rkk 10:d9f1037f0cb0 77
rkk 10:d9f1037f0cb0 78 float dutycycle;
rkk 10:d9f1037f0cb0 79 float phi2;
rkk 10:d9f1037f0cb0 80 if (firstTime)
rkk 10:d9f1037f0cb0 81 {
rkk 10:d9f1037f0cb0 82 dutycycle = amp2*sin(ome2 * currentTime);
rkk 10:d9f1037f0cb0 83 firstTime = false;
rkk 10:d9f1037f0cb0 84 }
rkk 10:d9f1037f0cb0 85 else if(amp2 > 0.0) {
rkk 10:d9f1037f0cb0 86 phi2 = asin(amp1 * sin(ome1 * currentTime + phi1) / amp2 ) - (ome2*currentTime);
rkk 10:d9f1037f0cb0 87 dutycycle = amp2*sin(ome2 * currentTime + phi2);
rkk 10:d9f1037f0cb0 88 phi1 = phi2;
rkk 10:d9f1037f0cb0 89 } else {
rkk 10:d9f1037f0cb0 90 dutycycle = 0.0;
rkk 10:d9f1037f0cb0 91 }
rkk 10:d9f1037f0cb0 92 setpolarspeed(dutycycle);
rkk 10:d9f1037f0cb0 93
rkk 10:d9f1037f0cb0 94 //set previous values
rkk 10:d9f1037f0cb0 95 ome1 = ome2;
rkk 10:d9f1037f0cb0 96 amp1 = amp2;
rkk 10:d9f1037f0cb0 97
rkk 10:d9f1037f0cb0 98 return dutycycle;
sandwich 5:090ef6275773 99 }
sandwich 7:e005cfaff8d1 100
sandwich 7:e005cfaff8d1 101 void PololuMController::drive_rectangular(float currentTime, float amplitude, float frequency)
sandwich 7:e005cfaff8d1 102 {
sandwich 7:e005cfaff8d1 103 //setpolarspeed(dutyCycle*sin( 2.0* MATH_PI* f * currentTime));
sandwich 7:e005cfaff8d1 104 float sinRes = sin( 2.0* MATH_PI* frequency * currentTime);
sandwich 7:e005cfaff8d1 105
sandwich 7:e005cfaff8d1 106 float dutycycle = amplitude * sigm( sinRes);
sandwich 7:e005cfaff8d1 107 setpolarspeed(dutycycle);
sandwich 7:e005cfaff8d1 108 return;
sandwich 7:e005cfaff8d1 109 }