tim008 tim008 / Mbed 2 deprecated projekatRadar

Sumejja Porča Almir Husić

Dependencies:   mbed sMotor

main.cpp@2:0daf6588a802, 2014-05-26 (annotated)

Committer:
tim008
Date:
Mon May 26 16:19:57 2014 +0000
Revision:
2:0daf6588a802
Parent:
1:d3ca3671067a
Child:
3:7f61e6ae0184
software za motor

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 1:d3ca3671067a 23 int angle(0);
tim008 1:d3ca3671067a 24 float scale (1 / (numstep/360.0));
tim008 0:9464a1d8de8d 25
tim008 0:9464a1d8de8d 26 string ToString(float num)
tim008 0:9464a1d8de8d 27 {
tim008 2:0daf6588a802 28 if (num < 0.0)
tim008 2:0daf6588a802 29 return "-1.0";
tim008 0:9464a1d8de8d 30 char number [4];
tim008 0:9464a1d8de8d 31 number[0] = num/10 + '0';
tim008 0:9464a1d8de8d 32 number[1] = int(num) % 10 + '0';
tim008 0:9464a1d8de8d 33 number[2] = '.';
tim008 0:9464a1d8de8d 34 number[3] = int((num - int(num)) * 10) + '0';
tim008 0:9464a1d8de8d 35 string Result = number;
tim008 0:9464a1d8de8d 36 return Result;
tim008 0:9464a1d8de8d 37 }
tim008 0:9464a1d8de8d 38
tim008 0:9464a1d8de8d 39 void MoveMotor()
tim008 0:9464a1d8de8d 40 {
tim008 1:d3ca3671067a 41 motor.step(int(numstep / 360.0 * scale * (angle % 360) + 0.5), direction, step_speed); // number of steps, direction, speed
tim008 1:d3ca3671067a 42 angle += 2;
tim008 0:9464a1d8de8d 43 index++;
tim008 0:9464a1d8de8d 44 index = index % buffer_size;
tim008 0:9464a1d8de8d 45 }
tim008 0:9464a1d8de8d 46
tim008 0:9464a1d8de8d 47 void CheckState()
tim008 0:9464a1d8de8d 48 {
tim008 0:9464a1d8de8d 49 if (pc.readable())
tim008 0:9464a1d8de8d 50 {
tim008 0:9464a1d8de8d 51 string command;
tim008 0:9464a1d8de8d 52 pc.scanf("%s", &command);
tim008 0:9464a1d8de8d 53 switch (state)
tim008 0:9464a1d8de8d 54 {
tim008 0:9464a1d8de8d 55 case Idle:
tim008 0:9464a1d8de8d 56 if (command == "Test")
tim008 0:9464a1d8de8d 57 pc.printf("Okay\n");
tim008 0:9464a1d8de8d 58 else if (command == "Start")
tim008 0:9464a1d8de8d 59 {
tim008 0:9464a1d8de8d 60 pc.printf("Started\n");
tim008 0:9464a1d8de8d 61 state = Working;
tim008 0:9464a1d8de8d 62 }
tim008 0:9464a1d8de8d 63
tim008 0:9464a1d8de8d 64 break;
tim008 0:9464a1d8de8d 65 case Working:
tim008 0:9464a1d8de8d 66 if (command == "Stop")
tim008 0:9464a1d8de8d 67 {
tim008 0:9464a1d8de8d 68 pc.printf("Stopped\n");
tim008 0:9464a1d8de8d 69 state = Idle;
tim008 0:9464a1d8de8d 70 }
tim008 0:9464a1d8de8d 71 break;
tim008 0:9464a1d8de8d 72 }
tim008 0:9464a1d8de8d 73 }
tim008 0:9464a1d8de8d 74 }
tim008 0:9464a1d8de8d 75
tim008 0:9464a1d8de8d 76 float InterpolatedValue( int i )
tim008 0:9464a1d8de8d 77 {
tim008 0:9464a1d8de8d 78 float alpha = (value - voltages[i]) / (voltages[i + 1] - voltages[i]);
tim008 0:9464a1d8de8d 79 return (1 - alpha)* voltages[i] + alpha*voltages[i + 1];
tim008 0:9464a1d8de8d 80 }
tim008 0:9464a1d8de8d 81
tim008 0:9464a1d8de8d 82 void GenerateValue()
tim008 0:9464a1d8de8d 83 {
tim008 0:9464a1d8de8d 84 string distance;
tim008 0:9464a1d8de8d 85
tim008 0:9464a1d8de8d 86 value = sensor;
tim008 0:9464a1d8de8d 87 if( value < 0.0001)
tim008 0:9464a1d8de8d 88 value = -1.0;
tim008 0:9464a1d8de8d 89 else
tim008 0:9464a1d8de8d 90 for( int i = 0; i < samples_num - 1; i++)
tim008 0:9464a1d8de8d 91 {
tim008 0:9464a1d8de8d 92 if( value > voltages[0])
tim008 0:9464a1d8de8d 93 {
tim008 0:9464a1d8de8d 94 value = 10;
tim008 0:9464a1d8de8d 95 break;
tim008 0:9464a1d8de8d 96 }
tim008 0:9464a1d8de8d 97 else if( value >= voltages[i] && value <= voltages[i + 1])
tim008 0:9464a1d8de8d 98 {
tim008 0:9464a1d8de8d 99 value = InterpolatedValue(i);
tim008 0:9464a1d8de8d 100 break;
tim008 0:9464a1d8de8d 101 }
tim008 0:9464a1d8de8d 102 }
tim008 0:9464a1d8de8d 103
tim008 0:9464a1d8de8d 104 distance = ToString(value);
tim008 0:9464a1d8de8d 105
tim008 0:9464a1d8de8d 106 pc.printf("%s\n",distance);
tim008 0:9464a1d8de8d 107 }
tim008 0:9464a1d8de8d 108 int main() {
tim008 0:9464a1d8de8d 109
tim008 0:9464a1d8de8d 110 while (1) {
tim008 0:9464a1d8de8d 111 CheckState();
tim008 0:9464a1d8de8d 112 if (state == Working)
tim008 0:9464a1d8de8d 113 {
tim008 0:9464a1d8de8d 114 GenerateValue();
tim008 0:9464a1d8de8d 115 MoveMotor();
tim008 0:9464a1d8de8d 116 wait_ms(40);
tim008 0:9464a1d8de8d 117 }
tim008 0:9464a1d8de8d 118 }
tim008 0:9464a1d8de8d 119 }