final project  1
sweep.c
Go to the documentation of this file.
1 
10 #include "main.h"
11 
12 volatile int overflow;
13 volatile int rise_time; // start time of the return pulse
14 volatile int fall_time; // end time of the return pulse
15 volatile int j;
16 
17 volatile float servo_angle;
18 
19 
20 int delta = 0; //raw sonar data
21 float distancePing = 0; //sonar distance
22 unsigned char angle = 0; //sweep angle
23 unsigned char tempAngle1 = 0; //first edge of obj
24 unsigned char tempAngle2 = 0; //second edge of obj
25 float tempDistance = 0; //distance to obj in cm
26 int Qval = 0; //quantized IR data
27 int Distance = 0; //IR distance data
28 int prevDist = 80; //previous IR distance
29 int i;
30 int count = 0; //number of found obj
31 unsigned char theta = 0; //angle width of obj
32 float width = 0;
33 
34 float distances[10];
35 float angles[10];
36 float widths[10];
37 
38 oi_t *sensor_data;
39 
41 
44 void sweep_init()
45 {
46  clock_timer_init3(); //initialize clock
47  servo_init(); //initialize servo motor
48  ping_init(); //initialize sonar
49  ADC_init(); //initialize IR
50  sensor_data = oi_alloc();
51  oi_init(sensor_data);
52  clock_timer_init(); //initialize clock for
53 }
54 
55 
57 
60 void sweep()
61 {
62  int l; //rows
63  int flag = 0;
64 
65  unsigned char note[1] = { 60 };
66  unsigned char length[1] = { 64 };
67  oi_loadSong(0, 1, note, length); //play C for 1 second
68  oi_play_song(0);
69 
70  for (l = 0; l < 5; l++)
71  {
72  distances[l] = 0.0;
73  angles[l] = 0.0;
74  widths[l] = 0.0;
75  }
76 
77  //the sweep of the servo motor. 2 degrees at a time
78  for (i = 0; i <= 90; i++)
79  {
80  angle = i * 2;
81 
82  servo_setAngle(angle);
83 
84  //get the pulse width
85  delta = ping_read();
86 
87  //convert pulse width to distance in cm
88  distancePing = time2dist(delta);
89 
90  //initiate SS1 conversion
91  ADC0_PSSI_R = ADC_PSSI_SS0;
92 
93  //wait for ADC conversion to be complete
94  while ((ADC0_RIS_R & ADC_RIS_INR0) == 0)
95  ;
96 
97  //grab result
98  Qval = ADC0_SSFIFO0_R;
99 
100  ADC0_SAC_R = 0x04; //16x hardware oversampling according to page 847
101  timer_waitMillis(25);
102 
103  Distance = 130885 * pow(Qval, -1.184); // equation for converting to cm
104 
105  //If distance read is a outlier then it gets put back to a reasonable number
106  if (Distance > 150)
107  {
108  Distance = 150;
109  }
110  if (Distance < 0)
111  {
112  Distance = 0; //distance can’t be negative
113  }
114 
115  //Checks if there is a significant distance change when object is detected and if its not at
116  //the very beginning
117  if (((prevDist - Distance) > 25) && angle > 5 && flag == 0)
118  {
119  tempAngle1 = angle; //keeps track of the first edge of the object
120  tempDistance = distancePing / 10.0;
121  flag = 1;
122  uart_sendStr("first edge\n\r");
123  }
124  //Check for the second edge of the object
125  else if (flag && ((Distance == 150) || (Distance - prevDist > 25)))
126  {
127  flag = 0;
128  tempAngle2 = angle; //keeps track of the angle where object ends
129  theta = tempAngle2 - tempAngle1; //angular width of object
130  width = tempDistance * tan((theta * (3.14 / 180.0))); //equation for width of object
131  tempAngle1 = 0;
132  count++; //keep track of number of objects detected
133 
134  uart_sendStr("second edge\n\r");
135 
136  distances[count - 1] = tempDistance;
137  angles[count - 1] = tempAngle2 - (theta);
138  widths[count - 1] = width;
139 
140  char obj[100];
141  if (theta > 2 && distances[count - 1] < 120) {
142  sprintf(obj, "dist: %.2f\tangl: %.2f\twdth: %.2f\ttheta: %d\n\r", distances[count - 1], angles[count - 1], widths[count - 1], theta);
143  uart_sendStr(obj);
144  }
145  }
146 
147  }
148  count = 0;
149  prevDist = Distance;
150 }
void sweep()
sweeps from 0 to 180 degrees, reads and sends data
Definition: sweep.c:60
void servo_setAngle(double degrees)
sets the servo angle
Definition: servo.c:115
the main code for the robot to interact with the user
double time2dist(int time)
convert a sonar pulse width time to a distance
Definition: ping.c:112
void ping_init()
initializes ping sensor
Definition: ping.c:55
int ping_read()
reads the response pulse from the ping sensor
Definition: ping.c:85
void ADC_init()
initializes the registers and timers needed for the ADC
Definition: adc.c:19
void sweep_init()
initializes the sweep modules
Definition: sweep.c:44
iRobot Create Sensor Data
void clock_timer_init3(void)
initializes a clock timer
Definition: ping.c:123
void clock_timer_init(void)
intializes the clock timer
Definition: servo.c:55
void servo_init()
initializes the servo motor
Definition: servo.c:90
void uart_sendStr(const char *data)
sends an entire string of character over uart 1 module
Definition: uart.c:75