tim008 tim008
/
projekatRadar
Sumejja Porča Almir Husić
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?
User | Revision | Line number | New 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 | } |