-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathSensors.c
147 lines (132 loc) · 3.98 KB
/
Sensors.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
#include "Sensors.h"
#include "SelfParkingCars.h"
void RotateFrontSensorFront()
{
ChangeDuty[BACK_SENSOR](sensorPositions[12]);
}
void AlignRightSensors()
{
ChangeDuty[FRONT_SENSOR](sensorPositions[10]);
ChangeDuty[BACK_SENSOR](sensorPositions[2]);
}
void TriggerFrontSensorMeasurement()
{
ODR10_GPIOD_ODR_bit = 1;
Delay_us(40);
ODR10_GPIOD_ODR_bit = 0;
Delay_ms(60);
}
void TriggerBackSensorMeasurement()
{
ODR11_GPIOD_ODR_bit = 1;
Delay_us(40);
ODR11_GPIOD_ODR_bit = 0;
Delay_ms(60);
}
// Sensors:
//
// Position
// 90 degree 2ms right
// 0 degrees 1.5ms middle
// -90 degrees 1ms left
// for servos http://akizukidenshi.com/download/ds/towerpro/SG90.pdf
void InitBackServoPWM_Timer10_CH1_PB8()
{
pwmPeriod[BACK_SENSOR] = PWM_TIM10_Init(INITIAL_SENSOR_FREQUENCY);
pwmDuty[BACK_SENSOR] = sensorPositions[0];
ChangeDuty[BACK_SENSOR](pwmDuty[BACK_SENSOR]);
PWM_TIM10_Start(_PWM_CHANNEL1, &_GPIO_MODULE_TIM10_CH1_PB8);
}
void InitFrontServoPWM_Timer11_CH1_PB9()
{
pwmPeriod[FRONT_SENSOR] = PWM_TIM11_Init(INITIAL_SENSOR_FREQUENCY);
pwmDuty[FRONT_SENSOR] = sensorPositions[0];
ChangeDuty[FRONT_SENSOR](pwmDuty[FRONT_SENSOR]); // 1ms = 6484 2 ms = 12 973
PWM_TIM11_Start(_PWM_CHANNEL1, &_GPIO_MODULE_TIM11_CH1_PB9);
}
// Front Sensor
// Echo PC4, Trigger PC11
// Back Sensor
// Echo PC3
unsigned long merenje, merenjee;
char buf[5];
// https://cdn.sparkfun.com/datasheets/Sensors/Proximity/HCSR04.pdf
// uS / 58 = centimeters
// Timer 2 is used (uses unsigned long - 32 bits)
void Timer()
{
RCC_APB1ENR.TIM2EN = 1;
TIM2_CR1.CEN = 0;
TIM2_PSC = 5; // Prescaller+1 (120 Mhz / prescaller+1 represents frequency of Timer calls)
TIM2_ARR = 65535;
}
// Initializes sensors and servo motors that are used to rotate sensors
void InitializeSensors()
{
Timer();
// Initialize servo motors rotating sensors
InitBackServoPWM_Timer10_CH1_PB8();
InitFrontServoPWM_Timer11_CH1_PB9();
// Initialize pins for triggering measurements
GPIO_Digital_Output(&GPIOD_BASE, _GPIO_PINMASK_11); // Front sensor Trigger: PD11
GPIO_Digital_Output(&GPIOD_BASE, _GPIO_PINMASK_10); // Back sensor Trigger : PD10
// Initialize Echo pins for receiving measurements from sensors
GPIO_Digital_Input(&GPIOC_BASE, _GPIO_PINMASK_3); // Back sensor Echo : PC3
GPIO_Digital_Input(&GPIOC_BASE, _GPIO_PINMASK_4); // Front sensor Echo: PC4
// Enable interrupts from echo channels
SYSCFGEN_bit = 1;
// Back Sensor | Front Sensor
// Pin: PC3 | PC4
EXTI_RTSR |= 0x8 | 0x10; // Set interrupt on Rising edge
EXTI_FTSR |= 0x8 | 0x10; // Set interrupt on Falling edge
EXTI_IMR |= 0x8 | 0x10; // Set mask
SYSCFG_EXTICR1 |= 0x2000; SYSCFG_EXTICR2 |= 0x2;
NVIC_IntEnable(IVT_INT_EXTI3); NVIC_IntEnable(IVT_INT_EXTI4);
}
double GetResultsInCM(unsigned long distance)
{
return distance/580.0;
}
double rez, rez2 ;
double GetFrontSensorDistance()
{
return rez2;
}
double GetBackSensorDistance()
{
return rez;
}
void FrontSensorEcho() iv IVT_INT_EXTI4 ics ICS_AUTO {
EXTI_PR.B4 = 1; // clear flag
ODR15_GPIOE_ODR_bit = 0;
if(IDR4_GPIOC_IDR_bit == 1)
{
TIM2_CR1.CEN = 1;
};
if(IDR4_GPIOC_IDR_bit == 0)
{
TIM2_CR1.CEN = 0;
merenje = TIM2_CNT;
TIM2_CNT = 0;
if (merenje >= 4000)
{
ODR15_GPIOE_ODR_bit = 0;
}
rez2 = GetResultsInCM(merenje);
}
}
void BackSensorEcho() iv IVT_INT_EXTI3 ics ICS_AUTO {
EXTI_PR.B3 = 1; // clear flag
ODR15_GPIOE_ODR_bit = 0;
if(IDR3_GPIOC_IDR_bit == 1)
{
TIM2_CR1.CEN = 1;
};
if(IDR3_GPIOC_IDR_bit == 0)
{
TIM2_CR1.CEN = 0;
merenjee = TIM2_CNT;
TIM2_CNT = 0;
rez = GetResultsInCM(merenjee);
}
}