using potmeters as setpoint
Dependencies: Encoder HIDScope MODSERIAL mbed
main.cpp@2:ee58b6e8eb67, 2015-09-23 (annotated)
- Committer:
- Vigilance88
- Date:
- Wed Sep 23 09:21:36 2015 +0000
- Revision:
- 2:ee58b6e8eb67
- Parent:
- 1:e0c4625bbbab
pcontrol test
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Vigilance88 | 0:4bfe85fb30ab | 1 | #include "mbed.h" |
Vigilance88 | 0:4bfe85fb30ab | 2 | #include "encoder.h" |
Vigilance88 | 2:ee58b6e8eb67 | 3 | #include "MODSERIAL.h" |
Vigilance88 | 0:4bfe85fb30ab | 4 | |
Vigilance88 | 2:ee58b6e8eb67 | 5 | volatile bool looptimerflag; |
Vigilance88 | 2:ee58b6e8eb67 | 6 | |
Vigilance88 | 2:ee58b6e8eb67 | 7 | void setlooptimerflag(void) |
Vigilance88 | 0:4bfe85fb30ab | 8 | { |
Vigilance88 | 2:ee58b6e8eb67 | 9 | looptimerflag = true; |
Vigilance88 | 2:ee58b6e8eb67 | 10 | } |
Vigilance88 | 2:ee58b6e8eb67 | 11 | |
Vigilance88 | 2:ee58b6e8eb67 | 12 | int main(){ |
Vigilance88 | 2:ee58b6e8eb67 | 13 | //VARIABLES |
Vigilance88 | 2:ee58b6e8eb67 | 14 | AnalogIn potmeter(A1); |
Vigilance88 | 2:ee58b6e8eb67 | 15 | AnalogIn potmeter2(A0); |
Vigilance88 | 2:ee58b6e8eb67 | 16 | DigitalIn button(D8); |
Vigilance88 | 2:ee58b6e8eb67 | 17 | MODSERIAL pc(USBTX,USBRX); |
Vigilance88 | 2:ee58b6e8eb67 | 18 | |
Vigilance88 | 2:ee58b6e8eb67 | 19 | Encoder motor1(D13,D12,true); // channel A and B from encoder, true - getSpeed works |
Vigilance88 | 2:ee58b6e8eb67 | 20 | PwmOut pwm_motor1(D6); // D4 and D5 = motor 2, D7 and D6 = motor 2 |
Vigilance88 | 2:ee58b6e8eb67 | 21 | DigitalOut dir_motor1(D7); // |
Vigilance88 | 2:ee58b6e8eb67 | 22 | |
Vigilance88 | 2:ee58b6e8eb67 | 23 | Encoder motor2(D10,D9,true); // channel A and B from encoder, true - getSpeed works |
Vigilance88 | 2:ee58b6e8eb67 | 24 | PwmOut pwm_motor2(D5); // D4 and D5 = motor 2, D7 and D6 = motor 2 |
Vigilance88 | 2:ee58b6e8eb67 | 25 | DigitalOut dir_motor2(D4); // |
Vigilance88 | 2:ee58b6e8eb67 | 26 | |
Vigilance88 | 2:ee58b6e8eb67 | 27 | // MOTOR1 |
Vigilance88 | 2:ee58b6e8eb67 | 28 | float goal; |
Vigilance88 | 2:ee58b6e8eb67 | 29 | float pwm_to_motor; |
Vigilance88 | 2:ee58b6e8eb67 | 30 | // MOTOR2 |
Vigilance88 | 2:ee58b6e8eb67 | 31 | float goal2; |
Vigilance88 | 2:ee58b6e8eb67 | 32 | float pwm_to_motor2; |
Vigilance88 | 2:ee58b6e8eb67 | 33 | |
Vigilance88 | 2:ee58b6e8eb67 | 34 | //CODE |
Vigilance88 | 0:4bfe85fb30ab | 35 | pc.baud(9600); |
Vigilance88 | 0:4bfe85fb30ab | 36 | |
Vigilance88 | 2:ee58b6e8eb67 | 37 | //pwm_motor1.write(0.2f); // Speed |
Vigilance88 | 2:ee58b6e8eb67 | 38 | //dir_motor1.write(1); // Direction |
Vigilance88 | 2:ee58b6e8eb67 | 39 | |
Vigilance88 | 2:ee58b6e8eb67 | 40 | Ticker looptimer; |
Vigilance88 | 2:ee58b6e8eb67 | 41 | looptimer.attach(setlooptimerflag,0.01); |
Vigilance88 | 2:ee58b6e8eb67 | 42 | |
Vigilance88 | 2:ee58b6e8eb67 | 43 | while (1) { |
Vigilance88 | 2:ee58b6e8eb67 | 44 | while(looptimerflag != true); |
Vigilance88 | 2:ee58b6e8eb67 | 45 | looptimerflag = false; |
Vigilance88 | 0:4bfe85fb30ab | 46 | |
Vigilance88 | 2:ee58b6e8eb67 | 47 | //MOTOR1 |
Vigilance88 | 2:ee58b6e8eb67 | 48 | goal = (potmeter.read()-0.5)*4200; |
Vigilance88 | 2:ee58b6e8eb67 | 49 | pc.printf("setpoint: %f, %d, %f \n\r", goal, motor1.getPosition(),motor1.getSpeed()); |
Vigilance88 | 2:ee58b6e8eb67 | 50 | pwm_to_motor = (goal - motor1.getPosition())*.001; |
Vigilance88 | 2:ee58b6e8eb67 | 51 | |
Vigilance88 | 2:ee58b6e8eb67 | 52 | if(pwm_to_motor > 0) |
Vigilance88 | 2:ee58b6e8eb67 | 53 | dir_motor1 = 0; |
Vigilance88 | 2:ee58b6e8eb67 | 54 | else |
Vigilance88 | 2:ee58b6e8eb67 | 55 | dir_motor1 = 1; |
Vigilance88 | 2:ee58b6e8eb67 | 56 | |
Vigilance88 | 2:ee58b6e8eb67 | 57 | pwm_motor1.write(abs(pwm_to_motor)); |
Vigilance88 | 2:ee58b6e8eb67 | 58 | |
Vigilance88 | 2:ee58b6e8eb67 | 59 | //MOTOR2 |
Vigilance88 | 2:ee58b6e8eb67 | 60 | goal2 = (potmeter2.read()-0.5)*4200; |
Vigilance88 | 2:ee58b6e8eb67 | 61 | //pc.printf("setpoint: %f, %d, %f \n\r", goal2, motor2.getPosition(),motor2.getSpeed()); |
Vigilance88 | 2:ee58b6e8eb67 | 62 | pwm_to_motor2 = (goal2 - motor2.getPosition())*.001; |
Vigilance88 | 2:ee58b6e8eb67 | 63 | |
Vigilance88 | 2:ee58b6e8eb67 | 64 | if(pwm_to_motor2 > 0) |
Vigilance88 | 2:ee58b6e8eb67 | 65 | dir_motor2 = 0; |
Vigilance88 | 2:ee58b6e8eb67 | 66 | else |
Vigilance88 | 2:ee58b6e8eb67 | 67 | dir_motor2 = 1; |
Vigilance88 | 2:ee58b6e8eb67 | 68 | |
Vigilance88 | 2:ee58b6e8eb67 | 69 | pwm_motor2.write(abs(pwm_to_motor2)); |
Vigilance88 | 2:ee58b6e8eb67 | 70 | |
Vigilance88 | 2:ee58b6e8eb67 | 71 | |
Vigilance88 | 0:4bfe85fb30ab | 72 | } |
Vigilance88 | 0:4bfe85fb30ab | 73 | } |