Diff: main.cpp

Revision:

5:43d5529fbe1e

Parent:

4:73f9fd797965

Child:

6:83c4801a027d

--- a/main.cpp	Wed Nov 20 15:57:00 2013 +0000
+++ b/main.cpp	Tue Mar 04 12:06:27 2014 +0000
@@ -1,10 +1,13 @@
 #include "PS2MS.h"
 #include "PS2MS_INIT.h"
 #include "mbed.h"
+#include "Servo.h"
 
 #define ENABLE_1 true
 #define ENABLE_2 true
 
+Servo myservo(p21);
+
 DigitalOut myled(LED1);
 DigitalInOut clk(p23);
 DigitalInOut dat(p22);
@@ -36,6 +39,15 @@
 
 int main()
 {
+
+    float range = 0.00085;
+    float position = 0.5;
+    float position_adj = 0.03;
+    
+    myservo.calibrate(range, 45.0); 
+    myservo = position;
+    printf("position = %.3f, range = +/-%0.5f\n", position, range, position_adj);
+
     printf("IMHERE START\n");
     int s1bytenum = 0;
     char s1bytes[3];
@@ -52,6 +64,16 @@
     sensorToPrint[0] = sensorToPrint[1] = false;
     
     while(1) {
+        if (pc.readable()) {
+            switch(pc.getc()) {
+            case '1': position = 0.0 + position_adj; break;
+            case '2': position = 0.5 + position_adj; break;
+            case '3': position = 1.0 + position_adj; break;
+            }
+            //printf("position = %.3f, range = +/-%0.5f\n", position, range, position_adj);
+            myservo = position;
+        }
+    
         if (ENABLE_1) {
             s1bytenum = process_sensor_input(s1c, s1bytenum, s1bytes, 0);
             s1c = sensor1.getc();