a

Dependents:   3servotest 1stcomp 3rdcompfixstart 2ndcomp ... more

Fork of Servo by Tk A

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers servo.cpp Source File

servo.cpp

00001 #include "mbed.h"
00002 
00003 PwmOut pwmarm(PC_6);
00004 PwmOut pwmhand(PC_8);
00005 PwmOut pwmbelt(PC_9);
00006 float PERIOD=20000;
00007 
00008 const float btime=2.2;
00009 
00010 void armdegree(int degree) {
00011     int i;
00012 
00013    pwmarm.period_ms(20);        //20ms
00014    
00015        degree=10;
00016 
00017        i=500+degree*1900/180;
00018        pwmarm.write(i/PERIOD); 
00019 }
00020 
00021 void handdegree(int degree) {
00022     int i;
00023 
00024    pwmarm.period_ms(20);        //20ms
00025    
00026        degree=10;
00027 
00028        i=500+degree*1900/180;
00029        pwmhand.write(i/PERIOD); 
00030        
00031 }
00032 
00033 void beltup(void){
00034 
00035    pwmarm.period_ms(20);        //20ms
00036        pwmbelt.write(0.028); 
00037        
00038 }
00039 
00040 void beltdown(void){
00041 
00042    pwmarm.period_ms(20);        //20ms
00043        pwmbelt.write(0.1); 
00044        
00045 }
00046 
00047 
00048 void beltstop(void){
00049 
00050    pwmarm.period_ms(20);        //20ms
00051     pwmbelt.write(0.073); 
00052        
00053 }
00054 
00055 void close_hand(void) {
00056      int i,degree;
00057 
00058     pwmhand.period_ms(20);        //20ms
00059     
00060         degree=137;
00061 
00062         i=500+degree*1900/180;
00063         pwmhand.write(i/PERIOD); 
00064         
00065         
00066 }
00067 
00068 void close_arm(void) {
00069     int i,degree;
00070 
00071    pwmarm.period_ms(20);        //20ms
00072    
00073        degree=170;
00074 
00075        i=500+degree*1900/180;
00076        pwmarm.write(i/PERIOD); 
00077        
00078        
00079 }
00080 
00081 
00082 void open_hand(void) {
00083     int i,degree;
00084 
00085    pwmhand.period_ms(20);        //20ms
00086    
00087        degree=83;
00088 
00089        i=500+degree*1900/180;
00090        pwmhand.write(i/PERIOD); 
00091        
00092        
00093 }
00094 
00095 
00096 
00097 void open_arm(void) {
00098     int i,degree;
00099 
00100    pwmarm.period_ms(20);        //20ms
00101    
00102        degree=30;
00103 
00104        i=500+degree*1900/180;
00105        pwmarm.write(i/PERIOD); 
00106        
00107        
00108 }
00109 
00110 void lift(){
00111     
00112 beltup();
00113 wait(btime);
00114 beltstop();
00115     
00116 }