tim008 tim008 / Mbed 2 deprecated projekatRadar

Sumejja Porča Almir Husić

Dependencies:   mbed sMotor

main.cpp@0:9464a1d8de8d, 2014-05-26 (annotated)

Committer:
tim008
Date:
Mon May 26 00:06:51 2014 +0000
Revision:
0:9464a1d8de8d
Child:
1:d3ca3671067a
Sumejja Por?a;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tim008 0:9464a1d8de8d 1 #include "mbed.h"
tim008 0:9464a1d8de8d 2 #include "sMotor.h"
tim008 0:9464a1d8de8d 3 #include <string>
tim008 0:9464a1d8de8d 4
tim008 0:9464a1d8de8d 5 sMotor motor(dp9, dp10, dp11, dp13); // creates new stepper motor: IN1, IN2, IN3, IN4
tim008 0:9464a1d8de8d 6 Serial pc(USBTX, USBRX);
tim008 0:9464a1d8de8d 7
tim008 0:9464a1d8de8d 8 int step_speed = 1200 ; // set default motor speed
tim008 0:9464a1d8de8d 9 int numstep = 512 ; // defines full turn of 360 degree
tim008 0:9464a1d8de8d 10 int direction = 0; //0 for right, 1 for left
tim008 0:9464a1d8de8d 11
tim008 0:9464a1d8de8d 12 AnalogIn sensor(dp23);
tim008 0:9464a1d8de8d 13
tim008 0:9464a1d8de8d 14 enum State {Idle, Working};
tim008 0:9464a1d8de8d 15
tim008 0:9464a1d8de8d 16 State state = Idle;
tim008 0:9464a1d8de8d 17 int index = 0;
tim008 0:9464a1d8de8d 18 int buffer_size = 300;
tim008 0:9464a1d8de8d 19 int samples_num = 13;
tim008 0:9464a1d8de8d 20 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};
tim008 0:9464a1d8de8d 21 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};
tim008 0:9464a1d8de8d 22 float value;
tim008 0:9464a1d8de8d 23 float angle(0.0);
tim008 0:9464a1d8de8d 24
tim008 0:9464a1d8de8d 25 string ToString(float num)
tim008 0:9464a1d8de8d 26 {
tim008 0:9464a1d8de8d 27 char number [4];
tim008 0:9464a1d8de8d 28 number[0] = num/10 + '0';
tim008 0:9464a1d8de8d 29 number[1] = int(num) % 10 + '0';
tim008 0:9464a1d8de8d 30 number[2] = '.';
tim008 0:9464a1d8de8d 31 number[3] = int((num - int(num)) * 10) + '0';
tim008 0:9464a1d8de8d 32 string Result = number;
tim008 0:9464a1d8de8d 33 return Result;
tim008 0:9464a1d8de8d 34 }
tim008 0:9464a1d8de8d 35
tim008 0:9464a1d8de8d 36 void MoveMotor()
tim008 0:9464a1d8de8d 37 {
tim008 0:9464a1d8de8d 38 motor.step(int(numstep / 360.0 * (angle - (angle / 360)* 360.0) + 0.5), direction, step_speed); // number of steps, direction, speed
tim008 0:9464a1d8de8d 39 angle += 1.2;
tim008 0:9464a1d8de8d 40 index++;
tim008 0:9464a1d8de8d 41 index = index % buffer_size;
tim008 0:9464a1d8de8d 42 }
tim008 0:9464a1d8de8d 43
tim008 0:9464a1d8de8d 44 void CheckState()
tim008 0:9464a1d8de8d 45 {
tim008 0:9464a1d8de8d 46 if (pc.readable())
tim008 0:9464a1d8de8d 47 {
tim008 0:9464a1d8de8d 48 string command;
tim008 0:9464a1d8de8d 49 pc.scanf("%s", &command);
tim008 0:9464a1d8de8d 50 switch (state)
tim008 0:9464a1d8de8d 51 {
tim008 0:9464a1d8de8d 52 case Idle:
tim008 0:9464a1d8de8d 53 if (command == "Test")
tim008 0:9464a1d8de8d 54 pc.printf("Okay\n");
tim008 0:9464a1d8de8d 55 else if (command == "Start")
tim008 0:9464a1d8de8d 56 {
tim008 0:9464a1d8de8d 57 pc.printf("Started\n");
tim008 0:9464a1d8de8d 58 state = Working;
tim008 0:9464a1d8de8d 59 }
tim008 0:9464a1d8de8d 60
tim008 0:9464a1d8de8d 61 break;
tim008 0:9464a1d8de8d 62 case Working:
tim008 0:9464a1d8de8d 63 if (command == "Stop")
tim008 0:9464a1d8de8d 64 {
tim008 0:9464a1d8de8d 65 pc.printf("Stopped\n");
tim008 0:9464a1d8de8d 66 state = Idle;
tim008 0:9464a1d8de8d 67 }
tim008 0:9464a1d8de8d 68 break;
tim008 0:9464a1d8de8d 69 }
tim008 0:9464a1d8de8d 70 }
tim008 0:9464a1d8de8d 71 }
tim008 0:9464a1d8de8d 72
tim008 0:9464a1d8de8d 73 float InterpolatedValue( int i )
tim008 0:9464a1d8de8d 74 {
tim008 0:9464a1d8de8d 75 float alpha = (value - voltages[i]) / (voltages[i + 1] - voltages[i]);
tim008 0:9464a1d8de8d 76 return (1 - alpha)* voltages[i] + alpha*voltages[i + 1];
tim008 0:9464a1d8de8d 77 }
tim008 0:9464a1d8de8d 78
tim008 0:9464a1d8de8d 79 void GenerateValue()
tim008 0:9464a1d8de8d 80 {
tim008 0:9464a1d8de8d 81 string distance;
tim008 0:9464a1d8de8d 82
tim008 0:9464a1d8de8d 83 value = sensor;
tim008 0:9464a1d8de8d 84 if( value < 0.0001)
tim008 0:9464a1d8de8d 85 value = -1.0;
tim008 0:9464a1d8de8d 86 else
tim008 0:9464a1d8de8d 87 for( int i = 0; i < samples_num - 1; i++)
tim008 0:9464a1d8de8d 88 {
tim008 0:9464a1d8de8d 89 if( value > voltages[0])
tim008 0:9464a1d8de8d 90 {
tim008 0:9464a1d8de8d 91 value = 10;
tim008 0:9464a1d8de8d 92 break;
tim008 0:9464a1d8de8d 93 }
tim008 0:9464a1d8de8d 94 else if( value >= voltages[i] && value <= voltages[i + 1])
tim008 0:9464a1d8de8d 95 {
tim008 0:9464a1d8de8d 96 value = InterpolatedValue(i);
tim008 0:9464a1d8de8d 97 break;
tim008 0:9464a1d8de8d 98 }
tim008 0:9464a1d8de8d 99 }
tim008 0:9464a1d8de8d 100
tim008 0:9464a1d8de8d 101 distance = ToString(value);
tim008 0:9464a1d8de8d 102
tim008 0:9464a1d8de8d 103 pc.printf("%s\n",distance);
tim008 0:9464a1d8de8d 104 }
tim008 0:9464a1d8de8d 105 int main() {
tim008 0:9464a1d8de8d 106
tim008 0:9464a1d8de8d 107 while (1) {
tim008 0:9464a1d8de8d 108 CheckState();
tim008 0:9464a1d8de8d 109 if (state == Working)
tim008 0:9464a1d8de8d 110 {
tim008 0:9464a1d8de8d 111 GenerateValue();
tim008 0:9464a1d8de8d 112 MoveMotor();
tim008 0:9464a1d8de8d 113 wait_ms(40);
tim008 0:9464a1d8de8d 114 }
tim008 0:9464a1d8de8d 115 }
tim008 0:9464a1d8de8d 116 }