Infrared Radar with Sharp Distance Sensor

1void setup()
2{
3  Serial.begin(115200);
4}
5 
6void loop()
7{
8   float adcVal;
9   
10  // Read sharp sensor analog value and use a simple filter   on it
11  adcVal = 0.0f;
12  for (int i = 0; i < 50; i++)
13  {
14    adcVal   += analogRead(PA0);
15    delay(10);
16  }
17  adcVal /= 50.0f;
18 
19  //   Print ADC Value
20  Serial.println(adcVal);
21}

1void setup()
2{
3  Serial.begin(115200);
4}
5 
6void loop()
7{
8  float adcVal;
9   
10  // Read sharp sensor analog value and use a simple filter  on it
11  adcVal = 0.0f;
12  for (int i = 0; i < 50; i++)
13  {
14    adcVal  += analogRead(PA0);
15    delay(10);
16  }
17  adcVal /= 50.0f;
18 
19  //  Print ADC Value
20  Serial.println(adcVal);
21}

1#include <Servo.h>
2 
3// ------------------------------------------------------------
4//  User parameters
5// ------------------------------------------------------------
6//  Define pin where Sharp sensor is located.
7#define SHARP_PIN       PA0
8// Define  pin where servo is located.
9#define SERVO_PIN       PA9
10// Define the maximum  distance you want to measure in [cm].
11#define DISTANCE_MAX    150
12// Define  the angle value that servo will step between measurements.
13#define SERVO_STEP      2
14// Define the delay in [ms] between measurements.
15#define MEAS_DELAY_MS   30
16 
17 
18// ------------------------------------------------------------
19//  Message handling
20// ------------------------------------------------------------
21//  Sync value in message header
22#define MSG_SYNC                (0x55AA)
23// Start  of checksum calculation in message header
24#define MSG_CHECKSUM_START      (0x55)
25//  Maximum size of a message
26#define MSG_MAX_SIZE            (64)
27 
28// Header  of message structure
29typedef struct
30{
31    uint16_t sync;
32    uint8_t  checkSum;
33    uint8_t cmd;
34} S_MSG_HEADER;
35 
36// Message: Parameters
37typedef  struct
38{
39    uint16_t distanceMax;
40    uint16_t sampleNum;
41} S_MESSAGE_PARAMETERS;
42  
43// Message: Obstacle
44typedef struct
45{
46    uint16_t angle;
47    uint16_t  distance;
48} S_MESSAGE_OBSTACLE;
49 
50// Message identifiers
51typedef enum
52{
53    eMESSAGE_PARAMETERS,
54    eMESSAGE_OBSTACLE,
55} E_MESSAGE_CMD;
56 
57//  Function to send a message to PC Application
58void sendMessage(uint8_t cmd, void*  data)
59{
60  // Structure for message to send
61  static struct
62  {
63    S_MSG_HEADER  header;
64    uint8_t data[MSG_MAX_SIZE];
65  } sendData;
66  uint8_t msgSize;
67  
68  // Get size of data based on command
69  switch (cmd)
70  {
71    case  eMESSAGE_PARAMETERS:
72      msgSize = sizeof(S_MESSAGE_PARAMETERS);
73      break;
74    case eMESSAGE_OBSTACLE:
75      msgSize = sizeof(S_MESSAGE_OBSTACLE);
76      break;
77  }
78 
79  // Fill header of message
80  sendData.header.cmd = cmd;
81  sendData.header.sync  = MSG_SYNC;
82  sendData.header.checkSum = MSG_CHECKSUM_START;
83   
84  // Fill  data bytes
85  for (uint8_t i = 0; i < msgSize; i++)
86  {
87    sendData.data[i]  = ((uint8_t*)(data))[i];
88    sendData.header.checkSum += sendData.data[i];
89  }
90   
91  // Send composed message to PC
92  Serial.write((uint8_t*)&sendData,  sizeof(S_MSG_HEADER) + msgSize);
93}
94 
95// ------------------------------------------------------------
96//  Distance calculation
97// ------------------------------------------------------------
98//  Structure to store ADC Value - Distance pairs
99typedef struct {
100  float distance;
101  float adcVal;
102} S_DIST_ADC_MAP;
103 
104// TODO
105// ADC Value - Distance  pairs
106// Fill it measured SHARP Sensor characteristic values.
107S_DIST_ADC_MAP  distAdcMap[] = {
108  {12,  3599},
109  {20,  3122},
110  {30,  2295},
111  {40,  1733},
112  {50,  1381},
113  {60,  1191},
114  {70,  1000},
115  {80,  855},
116  {90,  751},
117  {100, 662},
118  {110, 606},
119  {120, 559},
120  {130, 517},
121  {140, 465},
122  {150, 418},
123};
124 
125// Function to get distance [cm] from  measured ADC Value
126float getDistance(float adcVal)
127{
128  float distance =  DISTANCE_MAX;
129 
130  // Linear interpolation from measured ADC value and MAP.
131  for (int i = 1; i < (sizeof(distAdcMap)/sizeof(S_DIST_ADC_MAP)); i++)
132  {
133    if (adcVal > distAdcMap[i].adcVal)
134    {
135      float factor = (adcVal  - distAdcMap[i].adcVal)/(distAdcMap[i-1].adcVal - distAdcMap[i].adcVal);
136      distance  = factor * (distAdcMap[i-1].distance - distAdcMap[i].distance) + distAdcMap[i].distance;
137      break;
138    }
139  }
140 
141  return distance;
142}
143 
144// ------------------------------------------------------------
145//  Application
146// ------------------------------------------------------------
147//  Global variables
148Servo myServo;
149int servoVal = 0;
150int servoDir = SERVO_STEP;
151S_MESSAGE_OBSTACLE  msgObstacle;
152S_MESSAGE_PARAMETERS msgParameters;
153 
154void setup()
155{
156  Serial.begin(115200);
157  myServo.attach(SERVO_PIN);
158  myServo.write(servoVal);
159  
160  msgParameters.distanceMax = DISTANCE_MAX;
161  msgParameters.sampleNum = (180/SERVO_STEP);
162}
163  
164void loop()
165{
166  float adcVal;
167  float distance;
168 
169  // Send parameters  at start
170  if (servoVal == 0)
171    sendMessage(eMESSAGE_PARAMETERS, &msgParameters);
172  
173  // Read sharp sensor analog value and use a simple filter on it.
174  adcVal  = 0.0f;
175  for (int i = 0; i < 10; i++)
176    adcVal += analogRead(SHARP_PIN);
177  adcVal /= 10.0f;
178  distance = getDistance(adcVal);
179 
180  // Compose send  data structure from measured values
181  msgObstacle.angle    = servoVal;
182  msgObstacle.distance  = (uint16_t)distance;
183  sendMessage(eMESSAGE_OBSTACLE, &msgObstacle);
184 
185  // Set servo to next position
186  if (servoVal >= 180)
187    servoDir = -SERVO_STEP;
188  else if (servoVal <= 0)
189    servoDir = +SERVO_STEP;
190     
191  servoVal  += servoDir;
192  myServo.write(servoVal);
193 
194  // Wait until new measurement
195  delay(MEAS_DELAY_MS);
196}