Servo
Dependencies: mbed MODSERIAL Servo FastPWM
main.cpp
- Committer:
- s1923196
- Date:
- 2019-11-01
- Revision:
- 13:1a014f5b01df
- Parent:
- 12:515bacd56d07
File content as of revision 13:1a014f5b01df:
#include "Servo.h" #include "mbed.h" #include "FastPWM.h" #define M_PI 3.14159265358979323846 /* pi */ #include <math.h> Serial pc(USBTX, USBRX); Ticker servo_ticker; Servo mijnServo(D7); DigitalIn servo_button_pressed(SW3); //InterruptIn button(SW3); //int Position= mijnServo.read(); //bool servo_button_pressed; //void flag(void){ // servo_button_pressed=!servo_button_pressed; //} void servo() { double theta_s= 1300; // dit zijn allemaal waarden tussen de 500-2400 double theta_sout; // dit zijn allemaal waarden tussen de 500-2400 double q1=0; // 0.2*M_PI; // dit is in rad uit I.K double q2=0; //0.5*M_PI; // dit is in rad uit I.K double q1_ser; // dit is 500-2400 double q2_ser; // dit is 500-2400 q1_ser= q1*604.7887837; q2_ser= q2*604.7887837; if (servo_button_pressed.read()== false) { theta_sout=theta_s-q1_ser+q2_ser+400; if (theta_sout>=2400) { theta_sout=2400;} if (theta_sout<=500) { theta_sout=500;} else { theta_sout; } mijnServo.SetPosition(theta_sout); pc.printf("De servo staat op %f graden. in de klapstand\n\r", theta_sout); } else { theta_sout=theta_s-q1_ser+q2_ser; if (theta_sout>=2400) { theta_sout=2400;} if (theta_sout<=500) { theta_sout=500;} else { theta_sout; } mijnServo.SetPosition(theta_sout); pc.printf("De servo staat op %f graden.\n\r", theta_sout); } } int main(void) { mijnServo.Enable(1500,20000); servo(); servo_ticker.attach(servo,0.5); //button.fall(flag); while(1){} }