Celine van Loon / Mbed 2 deprecated Servoaansturing

Servo

Dependencies:   mbed MODSERIAL Servo FastPWM

main.cpp@6:e6125ef7705e, 2019-10-21 (annotated)

Committer:
s1923196
Date:
Mon Oct 21 15:13:14 2019 +0000
Revision:
6:e6125ef7705e
Parent:
5:74962b191242
Child:
7:464fb83c8cdf
servo met hoeken

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);
AnkePost 3:91b8945b659d 8
AnkePost 3:91b8945b659d 9 int main()
AnkePost 3:91b8945b659d 10 {
AnkePost 5:74962b191242 11 myservo.period(0.02f);
AnkePost 5:74962b191242 12 float t = 0;
AnkePost 5:74962b191242 13 while (true)
AnkePost 5:74962b191242 14 {
AnkePost 5:74962b191242 15 float ref = sin(t);
AnkePost 5:74962b191242 16 float pwm = 0.05 + 0.03 * ref; // maximale bereik: allebei 0.05
AnkePost 5:74962b191242 17 myservo.write(pwm);
AnkePost 5:74962b191242 18 t+=0.02; // nooit groter dan periode servomotor, zelfde geldt voor regel hieronder
AnkePost 5:74962b191242 19 wait(0.02);
AnkePost 5:74962b191242 20 }
s1923196 6:e6125ef7705e 21 }
s1923196 6:e6125ef7705e 22 float theta_s_out;
s1923196 6:e6125ef7705e 23
s1923196 6:e6125ef7705e 24
s1923196 6:e6125ef7705e 25 void servo_horizontal()
s1923196 6:e6125ef7705e 26 theta_s=theta_2_h+f; //f is de beginwaarde en theta_2_h is de waarde van INVERSE KINEM
s1923196 6:e6125ef7705e 27 if(0<=theta_s<=60)
s1923196 6:e6125ef7705e 28 {
s1923196 6:e6125ef7705e 29 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 30 }
s1923196 6:e6125ef7705e 31 if (60<theta_s<180)
s1923196 6:e6125ef7705e 32 {
s1923196 6:e6125ef7705e 33 theta_s_out= theta_s*0.0055555556
s1923196 6:e6125ef7705e 34 }
s1923196 6:e6125ef7705e 35 if (theta_s=>180)
s1923196 6:e6125ef7705e 36 {
s1923196 6:e6125ef7705e 37 theta_s_out=1
s1923196 6:e6125ef7705e 38 }
s1923196 6:e6125ef7705e 39
s1923196 6:e6125ef7705e 40
s1923196 6:e6125ef7705e 41 void servo_flippen() //voor het flippen is een waarde 0.2 gekozen dit kan veranderd worden
s1923196 6:e6125ef7705e 42 theta_s=theta_2_h+f; //f is de beginwaarde en theta_2_h is de waarde van INVERSE KINEM
s1923196 6:e6125ef7705e 43 if(0<=theta_s<=60)
s1923196 6:e6125ef7705e 44 {
s1923196 6:e6125ef7705e 45 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 46 }
s1923196 6:e6125ef7705e 47 if (60<theta_s<180)
s1923196 6:e6125ef7705e 48 {
s1923196 6:e6125ef7705e 49 theta_s_out= (theta_s*0.0055555556)-0.2
s1923196 6:e6125ef7705e 50
s1923196 6:e6125ef7705e 51 }
s1923196 6:e6125ef7705e 52 if (theta_s=>180)
s1923196 6:e6125ef7705e 53 {
s1923196 6:e6125ef7705e 54 theta_s_out=1-0.2
s1923196 6:e6125ef7705e 55 }
s1923196 6:e6125ef7705e 56
s1923196 6:e6125ef7705e 57
s1923196 6:e6125ef7705e 58
s1923196 6:e6125ef7705e 59
s1923196 6:e6125ef7705e 60 case controlling_position:
s1923196 6:e6125ef7705e 61 if (stateChanged)
s1923196 6:e6125ef7705e 62 {
s1923196 6:e6125ef7705e 63 servo_horizontal();
s1923196 6:e6125ef7705e 64 // functie waarbij de hoek gelijk blijft
s1923196 6:e6125ef7705e 65 stateChanged = false;
s1923196 6:e6125ef7705e 66 pc.printf("Servo hoek gelijk\r\n");
s1923196 6:e6125ef7705e 67 }
s1923196 6:e6125ef7705e 68 if (servo_button_pressed.read() == false) // Normaal waarde 1 bij indrukken, nu nul -> false
s1923196 6:e6125ef7705e 69 {
s1923196 6:e6125ef7705e 70 servo_flippen();
s1923196 6:e6125ef7705e 71 //currentState = flipping_spatula;
s1923196 6:e6125ef7705e 72 //stateChanged = true;
s1923196 6:e6125ef7705e 73 pc.printf("Moving to flipping spatula\r\n");
s1923196 6:e6125ef7705e 74 }
s1923196 6:e6125ef7705e 75 if (Emergency_button_pressed.read() == false) // Normaal waarde 1 bij indrukken, nu nul -> false
s1923196 6:e6125ef7705e 76 {
s1923196 6:e6125ef7705e 77 emergency();
s1923196 6:e6125ef7705e 78 }
s1923196 6:e6125ef7705e 79 break;