Jenhao Ou / SG90

simplify for single SG90

Dependents:   lidarproj

Fork of SG90 by Mathias Lyngklip

SG90.cpp@0:a62b163b1dbb, 2014-04-05 (annotated)

Committer:
iLyngklip
Date:
Sat Apr 05 07:56:43 2014 +0000
Revision:
0:a62b163b1dbb
Child:
1:93e46a70966f
dillerdasker

Who changed what in which revision?

UserRevisionLine numberNew contents of line
iLyngklip 0:a62b163b1dbb 1 #include "mbed.h"
iLyngklip 0:a62b163b1dbb 2 #include "SG90.h"
iLyngklip 0:a62b163b1dbb 3
iLyngklip 0:a62b163b1dbb 4
iLyngklip 0:a62b163b1dbb 5 //Initialize the position variables to neutral
iLyngklip 0:a62b163b1dbb 6
iLyngklip 0:a62b163b1dbb 7 servo::servo() : pwm1(PTA5), pwm2(PTA12), pwm3(PTA4), pwm4(PTA5)
iLyngklip 0:a62b163b1dbb 8 {
iLyngklip 0:a62b163b1dbb 9
iLyngklip 0:a62b163b1dbb 10 init();
iLyngklip 0:a62b163b1dbb 11 }
iLyngklip 0:a62b163b1dbb 12
iLyngklip 0:a62b163b1dbb 13
iLyngklip 0:a62b163b1dbb 14
iLyngklip 0:a62b163b1dbb 15 void servo::init() //Initialize all servo pwm's to a periode of 20ms
iLyngklip 0:a62b163b1dbb 16 {
iLyngklip 0:a62b163b1dbb 17 s1=s2=s3=s4=1400; //Initialize the position variables to neutral
iLyngklip 0:a62b163b1dbb 18 pwm1.period_ms(20);
iLyngklip 0:a62b163b1dbb 19 pwm1.pulsewidth_us(s1);
iLyngklip 0:a62b163b1dbb 20 pwm2.period_ms(20);
iLyngklip 0:a62b163b1dbb 21 pwm2.pulsewidth_us(s2);
iLyngklip 0:a62b163b1dbb 22 pwm3.period_ms(20);
iLyngklip 0:a62b163b1dbb 23 pwm3.pulsewidth_us(s3);
iLyngklip 0:a62b163b1dbb 24 pwm4.period_ms(20);
iLyngklip 0:a62b163b1dbb 25 pwm4.pulsewidth_us(s4);
iLyngklip 0:a62b163b1dbb 26 }
iLyngklip 0:a62b163b1dbb 27
iLyngklip 0:a62b163b1dbb 28 void servo::right(int a) //Lower the pulsewidth to make the servo turn clockwise
iLyngklip 0:a62b163b1dbb 29 {
iLyngklip 0:a62b163b1dbb 30 switch (a)
iLyngklip 0:a62b163b1dbb 31 {
iLyngklip 0:a62b163b1dbb 32 case 1:
iLyngklip 0:a62b163b1dbb 33 if (s1 >= 540) s1 = (s1 - 20);
iLyngklip 0:a62b163b1dbb 34 pwm1.pulsewidth_us(s1);
iLyngklip 0:a62b163b1dbb 35 break;
iLyngklip 0:a62b163b1dbb 36
iLyngklip 0:a62b163b1dbb 37 case 2:
iLyngklip 0:a62b163b1dbb 38 if (s2 >= 540)s2 =s2 -20;
iLyngklip 0:a62b163b1dbb 39 pwm2.pulsewidth_us(s2);
iLyngklip 0:a62b163b1dbb 40 break;
iLyngklip 0:a62b163b1dbb 41
iLyngklip 0:a62b163b1dbb 42 case 3:
iLyngklip 0:a62b163b1dbb 43 if (s3 >= 540)s3 = s3 -20;
iLyngklip 0:a62b163b1dbb 44 pwm3.pulsewidth_us(s3);
iLyngklip 0:a62b163b1dbb 45 break;
iLyngklip 0:a62b163b1dbb 46
iLyngklip 0:a62b163b1dbb 47 case 4:
iLyngklip 0:a62b163b1dbb 48 if (s4 >= 540)s4 = s4 -20;
iLyngklip 0:a62b163b1dbb 49 pwm4.pulsewidth_us(s4);
iLyngklip 0:a62b163b1dbb 50 break;
iLyngklip 0:a62b163b1dbb 51
iLyngklip 0:a62b163b1dbb 52 default :
iLyngklip 0:a62b163b1dbb 53 break;
iLyngklip 0:a62b163b1dbb 54 }
iLyngklip 0:a62b163b1dbb 55 }
iLyngklip 0:a62b163b1dbb 56
iLyngklip 0:a62b163b1dbb 57 void servo::left(int a) //Raise the pulsewidth to make the servo turn counter clockwise
iLyngklip 0:a62b163b1dbb 58 {
iLyngklip 0:a62b163b1dbb 59 switch (a)
iLyngklip 0:a62b163b1dbb 60 {
iLyngklip 0:a62b163b1dbb 61 case 1:
iLyngklip 0:a62b163b1dbb 62 if (s1 <=2300) s1 = s1 + 20;
iLyngklip 0:a62b163b1dbb 63 pwm1.pulsewidth_us(s1);
iLyngklip 0:a62b163b1dbb 64 break;
iLyngklip 0:a62b163b1dbb 65
iLyngklip 0:a62b163b1dbb 66 case 2:
iLyngklip 0:a62b163b1dbb 67 if (s2 <=2300)s2 =s2 +20;
iLyngklip 0:a62b163b1dbb 68 pwm2.pulsewidth_us(s2);
iLyngklip 0:a62b163b1dbb 69 break;
iLyngklip 0:a62b163b1dbb 70
iLyngklip 0:a62b163b1dbb 71 case 3:
iLyngklip 0:a62b163b1dbb 72 if (s2 <=2300)s3 = s3 +20;
iLyngklip 0:a62b163b1dbb 73 pwm3.pulsewidth_us(s3);
iLyngklip 0:a62b163b1dbb 74 break;
iLyngklip 0:a62b163b1dbb 75
iLyngklip 0:a62b163b1dbb 76 case 4:
iLyngklip 0:a62b163b1dbb 77 if (s2 <=2300)s4 = s4 +20;
iLyngklip 0:a62b163b1dbb 78 pwm4.pulsewidth_us(s4);
iLyngklip 0:a62b163b1dbb 79 break;
iLyngklip 0:a62b163b1dbb 80
iLyngklip 0:a62b163b1dbb 81 default :
iLyngklip 0:a62b163b1dbb 82 break;
iLyngklip 0:a62b163b1dbb 83 }
iLyngklip 0:a62b163b1dbb 84 }
iLyngklip 0:a62b163b1dbb 85 void servo::position(int a, int p) //Position control of the servor, a is the servo and p is the position in degrees
iLyngklip 0:a62b163b1dbb 86 {
iLyngklip 0:a62b163b1dbb 87 int pw;
iLyngklip 0:a62b163b1dbb 88 pw = 500 + (p*10);
iLyngklip 0:a62b163b1dbb 89 switch (a)
iLyngklip 0:a62b163b1dbb 90 {
iLyngklip 0:a62b163b1dbb 91 case 1:
iLyngklip 0:a62b163b1dbb 92 pwm1.pulsewidth_us(pw);
iLyngklip 0:a62b163b1dbb 93 break;
iLyngklip 0:a62b163b1dbb 94
iLyngklip 0:a62b163b1dbb 95 case 2:
iLyngklip 0:a62b163b1dbb 96
iLyngklip 0:a62b163b1dbb 97 pwm2.pulsewidth_us(pw);
iLyngklip 0:a62b163b1dbb 98 break;
iLyngklip 0:a62b163b1dbb 99
iLyngklip 0:a62b163b1dbb 100 case 3:
iLyngklip 0:a62b163b1dbb 101
iLyngklip 0:a62b163b1dbb 102 pwm3.pulsewidth_us(pw);
iLyngklip 0:a62b163b1dbb 103 break;
iLyngklip 0:a62b163b1dbb 104
iLyngklip 0:a62b163b1dbb 105 case 4:
iLyngklip 0:a62b163b1dbb 106
iLyngklip 0:a62b163b1dbb 107 pwm4.pulsewidth_us(pw);
iLyngklip 0:a62b163b1dbb 108 break;
iLyngklip 0:a62b163b1dbb 109
iLyngklip 0:a62b163b1dbb 110 default :
iLyngklip 0:a62b163b1dbb 111 break;
iLyngklip 0:a62b163b1dbb 112 }
iLyngklip 0:a62b163b1dbb 113
iLyngklip 0:a62b163b1dbb 114 }