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
148
|
#include <NAxisMotion.h>
#include <Wire.h>
#include "Belt.h"
#include "logs.h"
// Belt sensors
int PhotoSenspin1 = 0; // premier photo sensor
int PhotoSensReading1; //premier photo sensor
int PhotoSenspin2 = 1; // deuxieme photo sensor
int PhotoSensReading2; //deuxieme photo sensor
//Object that for the sensor
NAxisMotion mySensor;
//Flag to indicate if an interrupt was detected
bool intDetected = false;
//At a Range of 4g, the threshold
//is set at 39.05mg or 0.3830m/s2.
//This Range is the default for NDOF Mode
int threshold = 5;
//At a filter Bandwidth of 62.5Hz,
//the duration is 8ms.
//This Bandwidth is the default for NDOF Mode
int duration = 1;
bool anyMotion = true;
//To store the last streamed time stamp
unsigned long lastStreamTime = 0;
const int streamPeriod = 500;
Belt::Belt()
{
_pin_pressure_bottom = 0;
_pin_pressure_top = 1;
_pin_flexion = 2;
_pin_gyro_x = 3;
_pin_gyro_y = 4;
_pin_gyro_z = 5;
}
void Belt::read_pressure_top()
{
_pressure_top = analogRead(_pin_pressure_top);
}
void Belt::read_pressure_bottom()
{
_pressure_bottom = analogRead(_pin_pressure_bottom);
}
void Belt::read_flexion()
{
_flexion = analogRead(_pin_flexion);
}
void Belt::read_angle_x()
{
_angle_x = analogRead(_pin_gyro_x);
}
void Belt::read_angle_y()
{
_angle_y = analogRead(_pin_gyro_y);
}
void Belt::read_angle_z()
{
_angle_z = analogRead(_pin_gyro_z);
}
static Belt belt;
static void send_pressure_bottom(int pressure)
{
serial_printf("PRESSURE_BOTTOM:%d\n", pressure);
}
static void send_pressure_top(int pressure)
{
serial_printf("PRESSURE_TOP:%d\n", pressure);
}
static void send_flexion(int flexion)
{
serial_printf("FLEXION:%d\n", flexion);
}
static void send_angle_x(int angle)
{
serial_printf("ANGLE_X:%d\n", angle);
}
static void send_angle_y(int angle)
{
serial_printf("ANGLE_Y:%d\n", angle);
}
static void send_angle_z(int angle)
{
serial_printf("ANGLE_Z:%d\n", angle);
}
void setup()
{
Serial.begin(115200); //Initialize the Serial Port to view information on the Serial Monitor
I2C.begin(); //Initialize I2C communication to the let the library communicate with the sensor.
mySensor.initSensor(); //The I2C Address can be changed here inside this function in the library
mySensor.setOperationMode(OPERATION_MODE_NDOF); //Can be configured to other operation modes as desired
mySensor.setUpdateMode(MANUAL); //The default is AUTO. Changing to MANUAL requires calling the relevant update functions prior to calling the read functions
//Setting to MANUAL requires fewer reads to the sensor
}
void loop()
{
if ((millis() - lastStreamTime) < streamPeriod) {
return;
}
lastStreamTime = millis();
mySensor.updateEuler(); //Update the Euler data into the structure of the object
mySensor.updateCalibStatus(); //Update the Calibration Status
PhotoSensReading1 = analogRead(PhotoSenspin1);
PhotoSensReading2 = analogRead(PhotoSenspin2);
/* fetch the values from the sensors */
belt.read_pressure_bottom();
belt.read_pressure_top();
belt.read_flexion();
belt.read_angle_x();
belt.read_angle_y();
belt.read_angle_z();
int heading = mySensor.readEulerHeading();
int pitch = mySensor.readEulerPitch();
int roll = mySensor.readEulerRoll();
serial_printf("pitch: [%d]\n", pitch);
//pitch = map(pitch, 1, 1023, 90, 270);
serial_printf("pitch_mapped: [%d]\n", pitch);
serial_puts("-------");
serial_printf("roll: [%d]\n", roll);
//roll = map(roll, 1, 1023, 90, 270);
serial_printf("roll_mapped: [%d]\n", roll);
serial_puts("-------");
serial_printf("heading: [%d]\n", heading);
//heading = map(heading, 1, 1023, 90, 270);
serial_printf("heading_mapped: [%d]\n", heading);
serial_printf("ANGLE_X:%d\n", pitch);
serial_printf("ANGLE_Y:%d\n", roll);
serial_printf("ANGLE_Z:%d\n", heading);
serial_printf("pressure_top: [%d]\n", PhotoSensReading1);
serial_printf("PRESSURE_TOP:%d\n", PhotoSensReading1);
serial_printf("PRESSURE_BOTTOM:%d\n", PhotoSensReading2);
}
|
