Diff: main.cpp

Revision:

0:9464a1d8de8d

Child:

1:d3ca3671067a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon May 26 00:06:51 2014 +0000
@@ -0,0 +1,116 @@
+#include "mbed.h"
+#include "sMotor.h"
+#include <string>
+ 
+sMotor motor(dp9, dp10, dp11, dp13); // creates new stepper motor: IN1, IN2, IN3, IN4
+Serial pc(USBTX, USBRX);
+
+int step_speed = 1200 ; // set default motor speed
+int numstep = 512 ; // defines full turn of 360 degree
+int direction = 0; //0 for right, 1 for left
+
+AnalogIn sensor(dp23);
+
+enum State {Idle, Working};
+
+State state = Idle;
+int index = 0;
+int buffer_size = 300;
+int samples_num = 13;
+float distances[13] = {15.0, 20.0, 25.0, 30.0, 35.0, 40.0, 45.0, 50.0, 55.0, 60.0, 65.0, 70.0, 75.0};
+float voltages[13] = {0.78, 0.72, 0.63, 0.55, 0.47, 0.43, 0.39, 0.36, 0.34, 0.33, 0.32, 0.311, 0.31};
+float value;
+float angle(0.0);
+
+string ToString(float num)
+{
+    char number [4];
+    number[0] = num/10 + '0';
+    number[1] = int(num) % 10 + '0';
+    number[2] = '.';
+    number[3] = int((num - int(num)) * 10) + '0';
+    string Result = number;
+    return Result;
+}
+
+void MoveMotor()
+{
+   motor.step(int(numstep / 360.0 * (angle - (angle / 360)* 360.0) + 0.5), direction, step_speed); // number of steps, direction, speed
+   angle += 1.2;
+   index++;
+   index = index % buffer_size;
+}
+
+void CheckState()
+{
+     if (pc.readable())
+    {
+       string command;
+       pc.scanf("%s", &command);
+       switch (state)
+     {
+        case Idle:
+         if (command == "Test")
+          pc.printf("Okay\n");
+         else if (command == "Start")
+            {
+                pc.printf("Started\n");
+                state = Working;
+            }
+
+       break;
+       case Working:
+        if (command == "Stop")
+        {
+          pc.printf("Stopped\n");
+          state = Idle;
+        }
+        break;
+    }
+    }
+}
+
+float InterpolatedValue( int i )
+{
+    float alpha = (value - voltages[i]) / (voltages[i + 1] - voltages[i]);
+    return (1 - alpha)* voltages[i] + alpha*voltages[i + 1];
+}
+
+void GenerateValue()
+{
+      string distance;
+
+      value = sensor;
+      if( value < 0.0001)
+      value = -1.0;
+      else
+      for( int i = 0; i < samples_num - 1; i++)
+      {
+          if( value > voltages[0])
+      {
+          value = 10;
+          break;
+      }
+      else if( value >= voltages[i] && value <= voltages[i + 1])
+      {
+          value = InterpolatedValue(i);
+          break;
+      }
+      }
+     
+     distance = ToString(value);
+      
+      pc.printf("%s\n",distance);    
+}
+int main() {
+    
+    while (1) {   
+                CheckState();
+                if (state == Working)
+                {
+                    GenerateValue();
+                    MoveMotor();
+                    wait_ms(40);             
+                }
+            }
+        }
\ No newline at end of file