Servo
Dependencies: mbed MODSERIAL Servo FastPWM
main.cpp@13:1a014f5b01df, 2019-11-01 (annotated)
- Committer:
- s1923196
- Date:
- Fri Nov 01 12:37:44 2019 +0000
- Revision:
- 13:1a014f5b01df
- Parent:
- 12:515bacd56d07
Servo
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
s1923196 | 10:1754b6220c7a | 1 | |
s1923196 | 10:1754b6220c7a | 2 | #include "Servo.h" |
AnkePost | 0:60a8a60074a7 | 3 | #include "mbed.h" |
s1923196 | 10:1754b6220c7a | 4 | #include "FastPWM.h" |
s1923196 | 12:515bacd56d07 | 5 | #define M_PI 3.14159265358979323846 /* pi */ |
s1923196 | 12:515bacd56d07 | 6 | #include <math.h> |
AnkePost | 0:60a8a60074a7 | 7 | Serial pc(USBTX, USBRX); |
s1923196 | 12:515bacd56d07 | 8 | Ticker servo_ticker; |
s1923196 | 10:1754b6220c7a | 9 | Servo mijnServo(D7); |
s1923196 | 12:515bacd56d07 | 10 | DigitalIn servo_button_pressed(SW3); |
s1923196 | 12:515bacd56d07 | 11 | //InterruptIn button(SW3); |
s1923196 | 11:bb036c288cd7 | 12 | //int Position= mijnServo.read(); |
s1923196 | 12:515bacd56d07 | 13 | //bool servo_button_pressed; |
s1923196 | 11:bb036c288cd7 | 14 | |
s1923196 | 12:515bacd56d07 | 15 | //void flag(void){ |
s1923196 | 12:515bacd56d07 | 16 | // servo_button_pressed=!servo_button_pressed; |
s1923196 | 12:515bacd56d07 | 17 | //} |
s1923196 | 11:bb036c288cd7 | 18 | |
s1923196 | 11:bb036c288cd7 | 19 | void servo() |
s1923196 | 11:bb036c288cd7 | 20 | { |
s1923196 | 13:1a014f5b01df | 21 | double theta_s= 1300; // dit zijn allemaal waarden tussen de 500-2400 |
s1923196 | 12:515bacd56d07 | 22 | double theta_sout; // dit zijn allemaal waarden tussen de 500-2400 |
s1923196 | 12:515bacd56d07 | 23 | double q1=0; // 0.2*M_PI; // dit is in rad uit I.K |
s1923196 | 12:515bacd56d07 | 24 | double q2=0; //0.5*M_PI; // dit is in rad uit I.K |
s1923196 | 11:bb036c288cd7 | 25 | double q1_ser; // dit is 500-2400 |
s1923196 | 11:bb036c288cd7 | 26 | double q2_ser; // dit is 500-2400 |
s1923196 | 11:bb036c288cd7 | 27 | |
s1923196 | 11:bb036c288cd7 | 28 | q1_ser= q1*604.7887837; |
s1923196 | 11:bb036c288cd7 | 29 | q2_ser= q2*604.7887837; |
s1923196 | 11:bb036c288cd7 | 30 | |
s1923196 | 12:515bacd56d07 | 31 | if (servo_button_pressed.read()== false) |
s1923196 | 11:bb036c288cd7 | 32 | { |
s1923196 | 11:bb036c288cd7 | 33 | theta_sout=theta_s-q1_ser+q2_ser+400; |
s1923196 | 12:515bacd56d07 | 34 | if (theta_sout>=2400) |
s1923196 | 12:515bacd56d07 | 35 | { theta_sout=2400;} |
s1923196 | 12:515bacd56d07 | 36 | if (theta_sout<=500) |
s1923196 | 12:515bacd56d07 | 37 | { theta_sout=500;} |
s1923196 | 12:515bacd56d07 | 38 | else |
s1923196 | 12:515bacd56d07 | 39 | { theta_sout; |
s1923196 | 12:515bacd56d07 | 40 | } |
s1923196 | 11:bb036c288cd7 | 41 | mijnServo.SetPosition(theta_sout); |
s1923196 | 12:515bacd56d07 | 42 | pc.printf("De servo staat op %f graden. in de klapstand\n\r", theta_sout); |
s1923196 | 11:bb036c288cd7 | 43 | } |
s1923196 | 13:1a014f5b01df | 44 | |
s1923196 | 11:bb036c288cd7 | 45 | else |
s1923196 | 11:bb036c288cd7 | 46 | { |
s1923196 | 11:bb036c288cd7 | 47 | theta_sout=theta_s-q1_ser+q2_ser; |
s1923196 | 12:515bacd56d07 | 48 | if (theta_sout>=2400) |
s1923196 | 12:515bacd56d07 | 49 | { theta_sout=2400;} |
s1923196 | 12:515bacd56d07 | 50 | if (theta_sout<=500) |
s1923196 | 12:515bacd56d07 | 51 | { theta_sout=500;} |
s1923196 | 12:515bacd56d07 | 52 | else { theta_sout; |
s1923196 | 12:515bacd56d07 | 53 | } |
s1923196 | 11:bb036c288cd7 | 54 | mijnServo.SetPosition(theta_sout); |
s1923196 | 11:bb036c288cd7 | 55 | pc.printf("De servo staat op %f graden.\n\r", theta_sout); |
s1923196 | 11:bb036c288cd7 | 56 | } |
s1923196 | 11:bb036c288cd7 | 57 | } |
s1923196 | 10:1754b6220c7a | 58 | int main(void) |
s1923196 | 12:515bacd56d07 | 59 | { |
s1923196 | 12:515bacd56d07 | 60 | mijnServo.Enable(1500,20000); |
s1923196 | 12:515bacd56d07 | 61 | servo(); |
s1923196 | 12:515bacd56d07 | 62 | servo_ticker.attach(servo,0.5); |
s1923196 | 12:515bacd56d07 | 63 | //button.fall(flag); |
s1923196 | 12:515bacd56d07 | 64 | while(1){} |
s1923196 | 11:bb036c288cd7 | 65 | } |