code to drive the motors to the right position
Dependencies: HIDScope QEI mbed
Fork of BMT-K9_potmeter_fade by
main.cpp@4:e481566a5b54, 2015-09-24 (annotated)
- Committer:
- ewoud
- Date:
- Thu Sep 24 09:43:33 2015 +0000
- Revision:
- 4:e481566a5b54
- Parent:
- 3:6a97e1d68511
- Child:
- 5:edac3771ede4
published, motor driver simple position by pot meter
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
vsluiter | 0:e300738b9507 | 1 | #include "mbed.h" |
ewoud | 1:f5b12280ea8a | 2 | #include "HIDScope.h" |
ewoud | 3:6a97e1d68511 | 3 | #include "QEI.h" |
vsluiter | 0:e300738b9507 | 4 | |
vsluiter | 0:e300738b9507 | 5 | // myled is an object of class PwmOut. It uses the LED_RED pin |
vsluiter | 0:e300738b9507 | 6 | // in human speech: myled is an output that can be controlled with PWM. LED_RED is the pin which is connected to the output |
ewoud | 2:6a4a2e355cd9 | 7 | PwmOut myled2(D5); |
ewoud | 2:6a4a2e355cd9 | 8 | PwmOut myled1(D6); |
ewoud | 2:6a4a2e355cd9 | 9 | |
ewoud | 3:6a97e1d68511 | 10 | DigitalOut motor1direction(D7); |
vsluiter | 0:e300738b9507 | 11 | // pot is an object of class AnalogIn. It uses the PTB0 pin |
vsluiter | 0:e300738b9507 | 12 | // in human speech: pot is an analog input. You can read the voltage on pin PTB0 |
ewoud | 2:6a4a2e355cd9 | 13 | AnalogIn pot1(A0); |
ewoud | 2:6a4a2e355cd9 | 14 | AnalogIn pot2(A1); |
vsluiter | 0:e300738b9507 | 15 | |
ewoud | 1:f5b12280ea8a | 16 | //HIDScope scope(1); |
ewoud | 1:f5b12280ea8a | 17 | Serial pc(USBTX, USBRX); |
ewoud | 3:6a97e1d68511 | 18 | QEI wheel (D12, D13, NC, 624); |
ewoud | 3:6a97e1d68511 | 19 | float lastpotread = 0; |
ewoud | 3:6a97e1d68511 | 20 | int countsPerRound = 32*131; |
ewoud | 3:6a97e1d68511 | 21 | float gototick; |
ewoud | 3:6a97e1d68511 | 22 | int currentpulses; |
ewoud | 4:e481566a5b54 | 23 | int errorsignal; |
ewoud | 4:e481566a5b54 | 24 | float Kf=0.2; |
vsluiter | 0:e300738b9507 | 25 | //start 'main' function. Should be done once in every C(++) program |
vsluiter | 0:e300738b9507 | 26 | int main() |
vsluiter | 0:e300738b9507 | 27 | { |
vsluiter | 0:e300738b9507 | 28 | //setup some stuff |
vsluiter | 0:e300738b9507 | 29 | //period of PWM signal is 10kHz. Every 100 microsecond a new PWM period is started |
ewoud | 1:f5b12280ea8a | 30 | myled1.period_ms(0.1); |
ewoud | 1:f5b12280ea8a | 31 | myled2.period_ms(0.1); |
ewoud | 3:6a97e1d68511 | 32 | myled1=0.5; |
ewoud | 2:6a4a2e355cd9 | 33 | //motor1=1; |
vsluiter | 0:e300738b9507 | 34 | //while 1 is unequal to zero. For humans: loop forever |
vsluiter | 0:e300738b9507 | 35 | while(1) { |
ewoud | 3:6a97e1d68511 | 36 | currentpulses=wheel.getPulses(); |
ewoud | 3:6a97e1d68511 | 37 | gototick = pot1.read()*countsPerRound; |
ewoud | 4:e481566a5b54 | 38 | errorsignal=gototick-currentpulses; |
ewoud | 3:6a97e1d68511 | 39 | if (lastpotread != pot1.read()){ |
ewoud | 3:6a97e1d68511 | 40 | lastpotread=pot1.read(); |
ewoud | 4:e481566a5b54 | 41 | pc.printf("potvalue: %f, position: %d, control speed: %f \n\r",gototick,currentpulses,errorsignal*Kf); |
ewoud | 3:6a97e1d68511 | 42 | } |
vsluiter | 0:e300738b9507 | 43 | |
ewoud | 4:e481566a5b54 | 44 | if (errorsignal > 0) |
ewoud | 3:6a97e1d68511 | 45 | { |
ewoud | 4:e481566a5b54 | 46 | motor1direction=0; |
ewoud | 4:e481566a5b54 | 47 | myled1=errorsignal*Kf; |
ewoud | 3:6a97e1d68511 | 48 | |
ewoud | 3:6a97e1d68511 | 49 | } |
ewoud | 3:6a97e1d68511 | 50 | else |
ewoud | 3:6a97e1d68511 | 51 | { |
ewoud | 3:6a97e1d68511 | 52 | motor1direction=1; |
ewoud | 4:e481566a5b54 | 53 | myled1=-errorsignal*Kf; |
ewoud | 3:6a97e1d68511 | 54 | } |
ewoud | 3:6a97e1d68511 | 55 | |
ewoud | 3:6a97e1d68511 | 56 | |
ewoud | 3:6a97e1d68511 | 57 | //myled1.write(pot1.read()); |
ewoud | 3:6a97e1d68511 | 58 | //myled2.write(pot2.read()); |
vsluiter | 0:e300738b9507 | 59 | //wait some time to give the LED output a few PWM cycles. Otherwise a new value is written before the previously set PWM period (of 100microseconds) is finished |
vsluiter | 0:e300738b9507 | 60 | //This loop executes at roughly 100Hz (1/0.01s) |
vsluiter | 0:e300738b9507 | 61 | wait(0.01); |
vsluiter | 0:e300738b9507 | 62 | } |
vsluiter | 0:e300738b9507 | 63 | } |
vsluiter | 0:e300738b9507 | 64 |