jetfishteam
the fish that looks like a jet
Dependencies: ADXL345 ADXL345_I2C IMUfilter ITG3200 mbed Servo
motor_controller.cpp@11:8ec915eb70f6, 2014-01-31 (annotated)
- Committer:
- rkk
- Date:
- Fri Jan 31 19:36:28 2014 +0000
- Revision:
- 11:8ec915eb70f6
- Parent:
- 10:d9f1037f0cb0
- Child:
- 12:7eeb29892625
added main controller and control amplitude and frequency of fish tail properly
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| sandwich | 0:ff9bc5f69c57 | 1 | #include "motor_controller.h" |
| rkk | 11:8ec915eb70f6 | 2 | // |
| rkk | 11:8ec915eb70f6 | 3 | //float sigm(float input) |
| rkk | 11:8ec915eb70f6 | 4 | //{ |
| rkk | 11:8ec915eb70f6 | 5 | // if (input>0) |
| rkk | 11:8ec915eb70f6 | 6 | // return 1; |
| rkk | 11:8ec915eb70f6 | 7 | // else if (input<0) |
| rkk | 11:8ec915eb70f6 | 8 | // return -1; |
| rkk | 11:8ec915eb70f6 | 9 | // else |
| rkk | 11:8ec915eb70f6 | 10 | // return 0; |
| rkk | 11:8ec915eb70f6 | 11 | //} |
| sandwich | 7:e005cfaff8d1 | 12 | |
| sandwich | 0:ff9bc5f69c57 | 13 | PololuMController::PololuMController(PinName pwmport, PinName A, PinName B) |
| rkk | 11:8ec915eb70f6 | 14 | :pwm(pwmport),outA(A),outB(B){ |
| rkk | 11:8ec915eb70f6 | 15 | outA.write(0); |
| rkk | 11:8ec915eb70f6 | 16 | outB.write(1); |
| rkk | 11:8ec915eb70f6 | 17 | // timestamp=0; |
| rkk | 11:8ec915eb70f6 | 18 | // ome1 = 0.0; |
| rkk | 11:8ec915eb70f6 | 19 | // amp1 = 0.0; |
| rkk | 11:8ec915eb70f6 | 20 | // phi1 = 0.0; |
| rkk | 11:8ec915eb70f6 | 21 | // firstTime = true; |
| rkk | 11:8ec915eb70f6 | 22 | |
| sandwich | 0:ff9bc5f69c57 | 23 | } |
| sandwich | 0:ff9bc5f69c57 | 24 | |
| sandwich | 0:ff9bc5f69c57 | 25 | |
| rkk | 11:8ec915eb70f6 | 26 | //void PololuMController::setspeed(float speed) |
| rkk | 11:8ec915eb70f6 | 27 | //{ |
| rkk | 11:8ec915eb70f6 | 28 | // pwm->write(speed); |
| rkk | 11:8ec915eb70f6 | 29 | // return; |
| rkk | 11:8ec915eb70f6 | 30 | //} |
| sandwich | 0:ff9bc5f69c57 | 31 | |
| sandwich | 0:ff9bc5f69c57 | 32 | void PololuMController::setpolarspeed(float speed) |
| sandwich | 0:ff9bc5f69c57 | 33 | { |
| sandwich | 0:ff9bc5f69c57 | 34 | if (speed>=0) |
| sandwich | 0:ff9bc5f69c57 | 35 | { |
| rkk | 11:8ec915eb70f6 | 36 | outA.write(0); |
| rkk | 11:8ec915eb70f6 | 37 | outB.write(1); |
| rkk | 11:8ec915eb70f6 | 38 | pwm.write(abs(speed)); |
| sandwich | 0:ff9bc5f69c57 | 39 | } |
| sandwich | 0:ff9bc5f69c57 | 40 | else |
| sandwich | 0:ff9bc5f69c57 | 41 | { |
| rkk | 11:8ec915eb70f6 | 42 | outA.write(1); |
| rkk | 11:8ec915eb70f6 | 43 | outB.write(0); |
| rkk | 11:8ec915eb70f6 | 44 | pwm.write(abs(speed)); |
| sandwich | 0:ff9bc5f69c57 | 45 | } |
| sandwich | 0:ff9bc5f69c57 | 46 | return; |
| sandwich | 0:ff9bc5f69c57 | 47 | } |
| sandwich | 0:ff9bc5f69c57 | 48 | |
| rkk | 11:8ec915eb70f6 | 49 | //void PololuMController::reverse() |
| rkk | 11:8ec915eb70f6 | 50 | //{ |
| rkk | 11:8ec915eb70f6 | 51 | // outA->write(!(outA->read())); |
| rkk | 11:8ec915eb70f6 | 52 | // outB->write(!(outB->read())); |
| rkk | 11:8ec915eb70f6 | 53 | // return; |
| rkk | 11:8ec915eb70f6 | 54 | //} |
| rkk | 10:d9f1037f0cb0 | 55 | |
| rkk | 11:8ec915eb70f6 | 56 | //float PololuMController::drive_sinusoidal(float currentTime, float amplitude, float frequency) |
| rkk | 11:8ec915eb70f6 | 57 | //{ |
| rkk | 11:8ec915eb70f6 | 58 | // |
| rkk | 11:8ec915eb70f6 | 59 | // //convert inputs |
| rkk | 11:8ec915eb70f6 | 60 | // float amp2; |
| rkk | 11:8ec915eb70f6 | 61 | // if(amplitude > 1.0) { |
| rkk | 11:8ec915eb70f6 | 62 | // amp2 = 1.0; |
| rkk | 11:8ec915eb70f6 | 63 | // } else if(amplitude < 0.0) { |
| rkk | 11:8ec915eb70f6 | 64 | // amp2 = 0.0; |
| rkk | 11:8ec915eb70f6 | 65 | // } else { |
| rkk | 11:8ec915eb70f6 | 66 | // amp2 = amplitude; |
| rkk | 11:8ec915eb70f6 | 67 | // } |
| rkk | 11:8ec915eb70f6 | 68 | // float ome2 = 2.0* MATH_PI * frequency; |
| rkk | 11:8ec915eb70f6 | 69 | // |
| rkk | 11:8ec915eb70f6 | 70 | // float dutycycle; |
| rkk | 11:8ec915eb70f6 | 71 | // float phi2; |
| rkk | 11:8ec915eb70f6 | 72 | // if (firstTime) |
| rkk | 11:8ec915eb70f6 | 73 | // { |
| rkk | 11:8ec915eb70f6 | 74 | // dutycycle = amp2*sin(ome2 * currentTime); |
| rkk | 11:8ec915eb70f6 | 75 | // firstTime = false; |
| rkk | 11:8ec915eb70f6 | 76 | // } |
| rkk | 11:8ec915eb70f6 | 77 | // else if(amp2 > 0.0) { |
| rkk | 11:8ec915eb70f6 | 78 | // phi2 = asin(amp1 * sin(ome1 * currentTime + phi1) / amp2 ) - (ome2*currentTime); |
| rkk | 11:8ec915eb70f6 | 79 | // dutycycle = amp2*sin(ome2 * currentTime + phi2); |
| rkk | 11:8ec915eb70f6 | 80 | // phi1 = phi2; |
| rkk | 11:8ec915eb70f6 | 81 | // } else { |
| rkk | 11:8ec915eb70f6 | 82 | // dutycycle = 0.0; |
| rkk | 11:8ec915eb70f6 | 83 | // } |
| rkk | 11:8ec915eb70f6 | 84 | // setpolarspeed(dutycycle); |
| rkk | 11:8ec915eb70f6 | 85 | // |
| rkk | 11:8ec915eb70f6 | 86 | // //set previous values |
| rkk | 11:8ec915eb70f6 | 87 | // ome1 = ome2; |
| rkk | 11:8ec915eb70f6 | 88 | // amp1 = amp2; |
| rkk | 11:8ec915eb70f6 | 89 | // |
| rkk | 11:8ec915eb70f6 | 90 | // return dutycycle; |
| rkk | 11:8ec915eb70f6 | 91 | //} |
| rkk | 11:8ec915eb70f6 | 92 | // |
| rkk | 11:8ec915eb70f6 | 93 | //void PololuMController::drive_rectangular(float currentTime, float amplitude, float frequency) |
| rkk | 11:8ec915eb70f6 | 94 | //{ |
| rkk | 11:8ec915eb70f6 | 95 | // //setpolarspeed(dutyCycle*sin( 2.0* MATH_PI* f * currentTime)); |
| rkk | 11:8ec915eb70f6 | 96 | // float sinRes = sin( 2.0* MATH_PI* frequency * currentTime); |
| rkk | 11:8ec915eb70f6 | 97 | // |
| rkk | 11:8ec915eb70f6 | 98 | // float dutycycle = amplitude * sigm( sinRes); |
| rkk | 11:8ec915eb70f6 | 99 | // setpolarspeed(dutycycle); |
| rkk | 11:8ec915eb70f6 | 100 | // return; |
| rkk | 11:8ec915eb70f6 | 101 | //} |