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@9:8dd7a76756e2, 2014-01-31 (annotated)

Committer:
rkk
Date:
Fri Jan 31 04:53:10 2014 +0000
Revision:
9:8dd7a76756e2
Parent:
8:0574a5db1fc4
Child:
10:d9f1037f0cb0
added more code to the sinusoidal driving

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 9:8dd7a76756e2 67 float dutycycle;
rkk 9:8dd7a76756e2 68 float phi2;
rkk 9:8dd7a76756e2 69 if(divisor > 0.0 && amplitude > 0.0)
rkk 9:8dd7a76756e2 70 {
rkk 9:8dd7a76756e2 71 phi2 = asin(amp1/amplitude*sin(ome1 * currentTime + phi1))/divisor;
rkk 9:8dd7a76756e2 72 dutycycle = amplitude*sin(ome2 * currentTime + phi2);
rkk 9:8dd7a76756e2 73 phi1 = phi2;
rkk 9:8dd7a76756e2 74 }
rkk 9:8dd7a76756e2 75 else{
rkk 9:8dd7a76756e2 76 dutycycle = 0.0;
rkk 9:8dd7a76756e2 77 }
rkk 9:8dd7a76756e2 78 setpolarspeed(dutycycle);
sandwich 7:e005cfaff8d1 79
rkk 9:8dd7a76756e2 80 //set previous values
rkk 9:8dd7a76756e2 81 ome1 = ome2;
rkk 9:8dd7a76756e2 82
rkk 9:8dd7a76756e2 83 amp1 = amplitude;
rkk 9:8dd7a76756e2 84
rkk 8:0574a5db1fc4 85 return;
sandwich 5:090ef6275773 86 }
sandwich 7:e005cfaff8d1 87
sandwich 7:e005cfaff8d1 88 void PololuMController::drive_rectangular(float currentTime, float amplitude, float frequency)
sandwich 7:e005cfaff8d1 89 {
sandwich 7:e005cfaff8d1 90 //setpolarspeed(dutyCycle*sin( 2.0* MATH_PI* f * currentTime));
sandwich 7:e005cfaff8d1 91 float sinRes = sin( 2.0* MATH_PI* frequency * currentTime);
sandwich 7:e005cfaff8d1 92
sandwich 7:e005cfaff8d1 93 float dutycycle = amplitude * sigm( sinRes);
sandwich 7:e005cfaff8d1 94 setpolarspeed(dutycycle);
sandwich 7:e005cfaff8d1 95 return;
sandwich 7:e005cfaff8d1 96 }