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?
User | Revision | Line number | New 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 | } |