Diff: localization.cpp

Revision:

6:ee872bf49be6

Parent:

5:dcce3f262830

Child:

7:fcdba69a0c22

--- a/localization.cpp	Wed Apr 20 01:02:55 2016 +0000
+++ b/localization.cpp	Thu Apr 21 02:33:18 2016 +0000
@@ -10,6 +10,7 @@
 BNO055 imu(p28, p27); 
 //ultrasonic mu1(p26, p25, .1, 1, &dist);
 //ultrasonic mu2(p30, p29, .1, 1, &dist);
+PwmOut pin24(p24);
 
 localization::localization(){
     X = 0;
@@ -17,6 +18,8 @@
 }
 
 void localization::reset(){
+    pin24.period_ms(20);
+    
     //mu1.startUpdates();//start mesuring the distance
     //mu2.startUpdates();
     imu.reset();
@@ -99,6 +102,17 @@
 }
 */
 
+void localization::servo(int degree){
+    /*Input have to be between 0 and 180*/
+    if (degree > 180)
+        degree = 180;
+    else if (degree < 0)
+        degree = 0;
+    int highTime_us = (int)((degree / 180.0) * 2000) + 500; // Minimum 500us, maximum 2500us
+    
+    pin24.pulsewidth_us(highTime_us);  //Set the duty cycle
+}
+
 int localization::getX(){
     return X;
 }