![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Servo
Dependencies: mbed MODSERIAL Servo FastPWM
main.cpp@7:464fb83c8cdf, 2019-10-25 (annotated)
- Committer:
- s1923196
- Date:
- Fri Oct 25 14:32:41 2019 +0000
- Revision:
- 7:464fb83c8cdf
- Parent:
- 6:e6125ef7705e
- Child:
- 8:3990a8c4ccea
servo met sinus
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
AnkePost | 0:60a8a60074a7 | 1 | #include "mbed.h" |
AnkePost | 5:74962b191242 | 2 | //#include "Servo.h" |
AnkePost | 5:74962b191242 | 3 | //#include "FastPWM.h" |
AnkePost | 5:74962b191242 | 4 | #include <math.h> |
AnkePost | 3:91b8945b659d | 5 | |
AnkePost | 0:60a8a60074a7 | 6 | Serial pc(USBTX, USBRX); |
AnkePost | 5:74962b191242 | 7 | PwmOut myservo(D5); |
s1923196 | 7:464fb83c8cdf | 8 | DigitalIn servo_button_pressed(D0); |
AnkePost | 3:91b8945b659d | 9 | |
AnkePost | 3:91b8945b659d | 10 | int main() |
AnkePost | 3:91b8945b659d | 11 | { |
AnkePost | 5:74962b191242 | 12 | myservo.period(0.02f); |
AnkePost | 5:74962b191242 | 13 | float t = 0; |
AnkePost | 5:74962b191242 | 14 | while (true) |
AnkePost | 5:74962b191242 | 15 | { |
AnkePost | 5:74962b191242 | 16 | float ref = sin(t); |
AnkePost | 5:74962b191242 | 17 | float pwm = 0.05 + 0.03 * ref; // maximale bereik: allebei 0.05 |
AnkePost | 5:74962b191242 | 18 | myservo.write(pwm); |
AnkePost | 5:74962b191242 | 19 | t+=0.02; // nooit groter dan periode servomotor, zelfde geldt voor regel hieronder |
AnkePost | 5:74962b191242 | 20 | wait(0.02); |
AnkePost | 5:74962b191242 | 21 | } |
s1923196 | 6:e6125ef7705e | 22 | } |
s1923196 | 6:e6125ef7705e | 23 | float theta_s_out; |
s1923196 | 6:e6125ef7705e | 24 | |
s1923196 | 6:e6125ef7705e | 25 | |
s1923196 | 6:e6125ef7705e | 26 | void servo_horizontal() |
s1923196 | 6:e6125ef7705e | 27 | theta_s=theta_2_h+f; //f is de beginwaarde en theta_2_h is de waarde van INVERSE KINEM |
s1923196 | 6:e6125ef7705e | 28 | if(0<=theta_s<=60) |
s1923196 | 6:e6125ef7705e | 29 | { |
s1923196 | 6:e6125ef7705e | 30 | theta_s_out=0.3 //f krijgt een vaste waarde tussen de range van 0-1. Dit getal is de rangewaarde voor f |
s1923196 | 6:e6125ef7705e | 31 | } |
s1923196 | 6:e6125ef7705e | 32 | if (60<theta_s<180) |
s1923196 | 6:e6125ef7705e | 33 | { |
s1923196 | 6:e6125ef7705e | 34 | theta_s_out= theta_s*0.0055555556 |
s1923196 | 6:e6125ef7705e | 35 | } |
s1923196 | 6:e6125ef7705e | 36 | if (theta_s=>180) |
s1923196 | 6:e6125ef7705e | 37 | { |
s1923196 | 6:e6125ef7705e | 38 | theta_s_out=1 |
s1923196 | 6:e6125ef7705e | 39 | } |
s1923196 | 6:e6125ef7705e | 40 | |
s1923196 | 6:e6125ef7705e | 41 | |
s1923196 | 6:e6125ef7705e | 42 | void servo_flippen() //voor het flippen is een waarde 0.2 gekozen dit kan veranderd worden |
s1923196 | 6:e6125ef7705e | 43 | theta_s=theta_2_h+f; //f is de beginwaarde en theta_2_h is de waarde van INVERSE KINEM |
s1923196 | 6:e6125ef7705e | 44 | if(0<=theta_s<=60) |
s1923196 | 6:e6125ef7705e | 45 | { |
s1923196 | 6:e6125ef7705e | 46 | theta_s_out=0.3-0.2 //f krijgt een vaste waarde tussen de range van 0-1. Dit getal is de rangewaarde voor f |
s1923196 | 6:e6125ef7705e | 47 | } |
s1923196 | 6:e6125ef7705e | 48 | if (60<theta_s<180) |
s1923196 | 6:e6125ef7705e | 49 | { |
s1923196 | 6:e6125ef7705e | 50 | theta_s_out= (theta_s*0.0055555556)-0.2 |
s1923196 | 6:e6125ef7705e | 51 | |
s1923196 | 6:e6125ef7705e | 52 | } |
s1923196 | 6:e6125ef7705e | 53 | if (theta_s=>180) |
s1923196 | 6:e6125ef7705e | 54 | { |
s1923196 | 6:e6125ef7705e | 55 | theta_s_out=1-0.2 |
s1923196 | 6:e6125ef7705e | 56 | } |
s1923196 | 6:e6125ef7705e | 57 | |
s1923196 | 6:e6125ef7705e | 58 | |
s1923196 | 6:e6125ef7705e | 59 | |
s1923196 | 6:e6125ef7705e | 60 | |
s1923196 | 6:e6125ef7705e | 61 | case controlling_position: |
s1923196 | 6:e6125ef7705e | 62 | if (stateChanged) |
s1923196 | 6:e6125ef7705e | 63 | { |
s1923196 | 6:e6125ef7705e | 64 | servo_horizontal(); |
s1923196 | 6:e6125ef7705e | 65 | // functie waarbij de hoek gelijk blijft |
s1923196 | 6:e6125ef7705e | 66 | stateChanged = false; |
s1923196 | 6:e6125ef7705e | 67 | pc.printf("Servo hoek gelijk\r\n"); |
s1923196 | 6:e6125ef7705e | 68 | } |
s1923196 | 6:e6125ef7705e | 69 | if (servo_button_pressed.read() == false) // Normaal waarde 1 bij indrukken, nu nul -> false |
s1923196 | 6:e6125ef7705e | 70 | { |
s1923196 | 6:e6125ef7705e | 71 | servo_flippen(); |
s1923196 | 6:e6125ef7705e | 72 | //currentState = flipping_spatula; |
s1923196 | 6:e6125ef7705e | 73 | //stateChanged = true; |
s1923196 | 6:e6125ef7705e | 74 | pc.printf("Moving to flipping spatula\r\n"); |
s1923196 | 6:e6125ef7705e | 75 | } |
s1923196 | 6:e6125ef7705e | 76 | if (Emergency_button_pressed.read() == false) // Normaal waarde 1 bij indrukken, nu nul -> false |
s1923196 | 6:e6125ef7705e | 77 | { |
s1923196 | 6:e6125ef7705e | 78 | emergency(); |
s1923196 | 6:e6125ef7705e | 79 | } |
s1923196 | 6:e6125ef7705e | 80 | break; |