Celine van Loon / Mbed 2 deprecated Servoaansturing

Servo

Dependencies:   mbed MODSERIAL Servo FastPWM

main.cpp@12:515bacd56d07, 2019-10-29 (annotated)

Committer:
s1923196
Date:
Tue Oct 29 11:00:26 2019 +0000
Revision:
12:515bacd56d07
Parent:
11:bb036c288cd7
Child:
13:1a014f5b01df
servo werkend

Who changed what in which revision?

UserRevisionLine numberNew 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 12:515bacd56d07 21 double theta_s= 500; // 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 11:bb036c288cd7 44 else
s1923196 11:bb036c288cd7 45 {
s1923196 11:bb036c288cd7 46 theta_sout=theta_s-q1_ser+q2_ser;
s1923196 12:515bacd56d07 47 if (theta_sout>=2400)
s1923196 12:515bacd56d07 48 { theta_sout=2400;}
s1923196 12:515bacd56d07 49 if (theta_sout<=500)
s1923196 12:515bacd56d07 50 { theta_sout=500;}
s1923196 12:515bacd56d07 51 else { theta_sout;
s1923196 12:515bacd56d07 52 }
s1923196 11:bb036c288cd7 53 mijnServo.SetPosition(theta_sout);
s1923196 11:bb036c288cd7 54 pc.printf("De servo staat op %f graden.\n\r", theta_sout);
s1923196 11:bb036c288cd7 55 }
s1923196 11:bb036c288cd7 56 }
s1923196 10:1754b6220c7a 57 int main(void)
s1923196 12:515bacd56d07 58 {
s1923196 12:515bacd56d07 59 mijnServo.Enable(1500,20000);
s1923196 12:515bacd56d07 60 servo();
s1923196 12:515bacd56d07 61 servo_ticker.attach(servo,0.5);
s1923196 12:515bacd56d07 62 //button.fall(flag);
s1923196 12:515bacd56d07 63 while(1){}
s1923196 11:bb036c288cd7 64 }