Celine van Loon / Mbed 2 deprecated Servoaansturing

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?

UserRevisionLine numberNew 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;