main.cpp

00001 #include "mbed.h"
00002 #include "sMotor.h"
00003 #include <string>
00004  
00005 sMotor motor(dp9, dp10, dp11, dp13); // creates new stepper motor: IN1, IN2, IN3, IN4
00006 Serial pc(USBTX, USBRX);
00007 
00008 int step_speed = 1200 ; // set default motor speed
00009 int numstep = 512 ; // defines full turn of 360 degree
00010 int direction = 0; //0 for right, 1 for left
00011 
00012 AnalogIn sensor(dp4);
00013 
00014 enum State {Idle, Working};
00015 
00016 State state = Idle;
00017 int index = 0;
00018 int buffer_size = 36;
00019 int samples_num = 13;
00020 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};
00021 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};
00022 float value;
00023 int angle(10);
00024     char number [4]={'0','0','0','0'};
00025 void ToString(float num)
00026 {
00027     number[0] = num/10 + '0';
00028     number[1] = int(num) % 10 + '0';
00029     number[2] = '.';
00030     number[3] = int((num - int(num)) * 10) + '0';
00031 
00032 }
00033 
00034 void MoveMotor()
00035 {
00036    motor.step(int(numstep / 360.0 * (angle  % 360) + 0.5), direction, step_speed); // number of steps, direction, speed
00037 }
00038 
00039 void CheckState()
00040 {     
00041      if (pc.readable())
00042     {
00043       string command;
00044        pc.scanf("%s",&command);
00045        if (command == "Start")
00046             {
00047                 state = Working;
00048             }
00049         else if (command == "Stop")
00050         {  state = Idle;
00051         }
00052          
00053     
00054     }
00055 }
00056 
00057 float InterpolatedValue( int i )
00058 {
00059     float alpha = (value - voltages[i]) / (voltages[i + 1] - voltages[i]);
00060     return (1 - alpha)* distances[i] + alpha*distances[i + 1];
00061 }
00062 
00063 void GenerateValue()
00064 {
00065 
00066       value = sensor;
00067       if( value < 0.0001)
00068       value = -1.0;
00069       else
00070       for( int i = 0; i < samples_num - 1; i++)
00071       {
00072           if( value > voltages[0])
00073       {
00074           value = 10;
00075           break;
00076       }
00077       else if( value >= voltages[i] && value <= voltages[i + 1])
00078       {
00079           value = InterpolatedValue(i);
00080           break;
00081       }
00082       }
00083      int valueInt = int(value + 0.5);
00084      pc.printf("%d\n", value); 
00085 }
00086 int main() {
00087     pc.attach(&CheckState);
00088     while (1) {   
00089    
00090     
00091                 if (state == Working)
00092                 {
00093                     
00094                     GenerateValue();
00095                     MoveMotor();
00096                     wait(1);             
00097                 }
00098          
00099         }
00100         }
00101