-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathSensor.py
129 lines (105 loc) · 5.12 KB
/
Sensor.py
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
# -*- coding: utf-8 -*-
"""
ECE183DB Sensor Classes
Created on Thu Apr 15 15:35:02 2021
@author: Ethan Uetrecht
"""
import numpy as np
# Basic sensor class, can be initialized with various properties, can 'read' values (apply noise to and discretize input truth values)
class Sensor:
resolution = 0
valueBounds = [0,0]
valueSpace = 0
rounding = ''
fullscale = 0
noiseVariance = 0
units = ''
def __init__(self, resolution, valueBounds, roundingType, noiseVariance, units):
self.resolution = resolution
self.valueBounds = valueBounds
self.rounding = roundingType
if(self.rounding not in ['floor', 'ceil', 'avg']):
raise ValueError('Invalid rounding type')
self.noiseVariance = noiseVariance
self.units = units
self.fullscale = self.valueBounds[1] - self.valueBounds[0]
self.valueSpace = np.linspace(self.valueBounds[0], self.valueBounds[1], self.resolution)
def apply_resolution(self, continuous_value):
discrete_value = 0
for i in np.arange(self.resolution):
if(self.valueSpace[i] > continuous_value and i > 0):
if(self.rounding == 'floor'):
discrete_value = self.valueSpace[i-1]
break
elif(self.rounding == 'ceil'):
discrete_value = self.valueSpace[i]
break
elif(self.rounding == 'avg'):
discrete_value = (self.valueSpace[i] + self.valueSpace[i-1])/2
break
if(self.valueSpace[-1] < continuous_value):
discrete_value = self.valueSpace[-1]
elif(self.valueSpace[0] > continuous_value):
discrete_value = self.valueSpace[0]
return discrete_value
def read_value(self, true_value):
noisy_value = true_value + np.random.normal(loc=0.0, scale=self.noiseVariance)
value = self.apply_resolution(noisy_value)
#value = noisy_value
return value, self.units
def check_units(self):
return self.units
# Unique: less default noise, value bounds of [-pi,pi]
class IMU(Sensor):
def __init__(self, resolution=1024, valueBounds=[-np.pi,np.pi], roundingType='floor', noiseVariance=0.001, units='radians'):
Sensor.__init__(self, resolution, valueBounds, roundingType, noiseVariance, units)
def read_value(self, true_value):
return Sensor.read_value(self,true_value)
def check_units(self):
return Sensor.check_units(self)
# Unique: actuator type selectable, some unit-actuator matching
class ActuatorPositionSensor(Sensor):
actuatorType = ''
def __init__(self, resolution=1024, valueBounds=[0,2*np.pi], roundingType='floor', noiseVariance=0.01, units='radians', actuatorType='rotational'):
Sensor.__init__(self, resolution, valueBounds, roundingType, noiseVariance, units)
if(actuatorType != 'linear' and actuatorType != 'rotational'):
raise ValueError('Invalid actuator type')
if((self.units == 'radians' or self.units == 'degrees') and actuatorType != 'rotational'):
raise ValueError('Units do not match actuator type')
def read_value(self, true_value):
return Sensor.read_value(self,true_value)
def check_units(self):
return Sensor.check_units(self)
# Unique: angular position rollover accounted for, previous value used for speed calculation
class WheelSpeedSensor(Sensor):
previousPosition = 'n/a'
previousTime = 'n/a'
previousSpeed = 'n/a'
def __init__(self, resolution=1024, valueBounds=[0,2*np.pi], roundingType='floor', noiseVariance=0.01, units='radians/s'):
Sensor.__init__(self, resolution, valueBounds, roundingType, noiseVariance, units)
def rollover_value(self, value):
while(value > self.valueSpace[-1]):
value -= self.fullscale
while(value < self.valueSpace[0]):
value += self.fullscale
return value
def read_value(self, true_position, current_time):
bounded_position = self.rollover_value(true_position)
position, units = Sensor.read_value(self,bounded_position)
if(self.previousPosition == 'n/a'):
self.previousPosition = position
self.previousTime = current_time
speed = 0
self.previousSpeed = speed
return speed, units
else:
if(position >= self.previousPosition and self.previousSpeed < 0):
speed = (position - self.fullscale - self.previousPosition)/(current_time - self.previousTime)
elif(position < self.previousPosition and self.previousSpeed >= 0):
speed = (position + self.fullscale - self.previousPosition)/(current_time - self.previousTime)
else:
speed = (position - self.previousPosition)/(current_time - self.previousTime)
self.previousSpeed = speed
return speed
def check_units(self):
return Sensor.check_units(self)