Head up with gimbals

1#include <Wire.h>
2#include <Adafruit_Sensor.h>
3#include <Adafruit_BNO055.h>
4#include <utility/imumaths.h>
5
6/* This driver reads raw data from the BNO055
7
8   Connections
9   ===========
10   Connect SCL to analog 5
11   Connect SDA to analog 4
12   Connect VDD to 3.3V DC
13   Connect GROUND to common ground
14
15   History
16   =======
17   2015/MAR/03  - First release (KTOWN)
18*/
19
20/* Set the delay between fresh samples */
21#define BNO055_SAMPLERATE_DELAY_MS (10)
22
23// Check I2C device address and correct line below (by default address is 0x29 or 0x28)
24//                                   id, address
25Adafruit_BNO055 bno = Adafruit_BNO055(-1, 0x28, &Wire);
26
27// Pour faire fonctionner le moteur en PWM
28int pwmPinA = 3;
29int pwmPinB = 11;
30int dirPinA = 12;
31int dirPinB = 13;
32//int titi = 100;
33
34/**************************************************************************/
35/*
36    Arduino setup function (automatically called at startup)
37*/
38/**************************************************************************/
39void setup(void)
40{
41//  pinMode(6, OUTPUT);
42  pinMode(pwmPinA, OUTPUT);
43  pinMode(pwmPinB, OUTPUT);
44  pinMode(dirPinA, OUTPUT);
45  pinMode(dirPinB, OUTPUT);
46  Serial.begin(115200);
47
48  while (!Serial) delay(10);  // wait for serial port to open!
49
50  Serial.println("Orientation Sensor Raw Data Test"); Serial.println("");
51
52  /* Initialise the sensor */
53  if(!bno.begin())
54  {
55    /* There was a problem detecting the BNO055 ... check your connections */
56    Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
57    while(1);
58  }
59
60  delay(1000);
61
62  /* Display the current temperature */
63  int8_t temp = bno.getTemp();
64//  Serial.print("Current Temperature: ");
65//  Serial.print(temp);
66//  Serial.println(" C");
67//  Serial.println("");
68
69  bno.setExtCrystalUse(true);
70
71//  Serial.println("Calibration status values: 0=uncalibrated, 3=fully calibrated");
72}
73
74/**************************************************************************/
75/*
76    Arduino loop function, called once 'setup' is complete (your own code
77    should go here)
78*/
79/**************************************************************************/
80void loop(void)
81{
82//  digitalWrite(6, HIGH);   // turn the LED on (HIGH is the voltage level)
83//  delay(1000);                       // wait for a second
84//  digitalWrite(6, LOW);    // turn the LED off by making the voltage LOW
85//  delay(1000);                       // wait for a second
86  
87  // Possible vector values can be:
88  // - VECTOR_ACCELEROMETER - m/s^2
89  // - VECTOR_MAGNETOMETER  - uT
90  // - VECTOR_GYROSCOPE     - rad/s
91  // - VECTOR_EULER         - degrees
92  // - VECTOR_LINEARACCEL   - m/s^2
93  // - VECTOR_GRAVITY       - m/s^2
94  imu::Vector<3> euler = bno.getVector(Adafruit_BNO055::VECTOR_EULER);
95/*
96  int eulerx = 0;
97  eulerx = euler.x();
98  if((euler.x() <= 360)&&(euler.x()) > 180) {
99    eulerx = euler.x()- 360;
100  }
101*/
102  int totoz = 3;
103  int totoy = 12;
104
105  /* Display the floating point data */
106//  Serial.print("X: ");
107//  Serial.print(euler.x());
108//  Serial.print(" Y: ");
109//  Serial.print(euler.y());
110//  Serial.print(" Z: ");
111//  Serial.print(euler.z());
112//  Serial.print("\t\n");
113
114/*
115  if(titi > 0) {
116    totoz = 3- 10* euler.z();
117    totoy = 13- 10* euler.y();
118    Serial.println(titi);
119    Serial.println(totoz);
120    Serial.println(totoy); 
121    titi--;
122  }
123*/
124  /*
125  // Quaternion data
126  imu::Quaternion quat = bno.getQuat();
127  Serial.print("qW: ");
128  Serial.print(quat.w(), 4);
129  Serial.print(" qX: ");
130  Serial.print(quat.x(), 4);
131  Serial.print(" qY: ");
132  Serial.print(quat.y(), 4);
133  Serial.print(" qZ: ");
134  Serial.print(quat.z(), 4);
135  Serial.print("\t\t");
136  */
137
138  /* Display calibration status for each sensor. */
139  uint8_t system, gyro, accel, mag = 0;
140  bno.getCalibration(&system, &gyro, &accel, &mag);
141//  Serial.print("CALIBRATION: Sys=");
142//  Serial.print(system, DEC);
143//  Serial.print(" Gyro=");
144//  Serial.print(gyro, DEC);
145//  Serial.print(" Accel=");
146//  Serial.print(accel, DEC);
147//  Serial.print(" Mag=");
148//  Serial.println(mag, DEC);
149
150  delay(BNO055_SAMPLERATE_DELAY_MS);
151
152//  int x=analogRead(A0);
153//  Serial.print("X Potentiometer: ");
154//  Serial.println(x);
155
156  Serial.println(totoz -1.1* euler.z());
157  Serial.println(totoy -1.1* euler.y());
158
159  //set work duty for the motor
160  //analogWrite(pwmPin, 1024 -x); // asservissement de la vitesse du moteur avec un potentiomètre
161  if(/*totoz */-1.1* euler.z() < 0) {
162//    totoz = 3;
163    digitalWrite(dirPinA, LOW);
164    analogWrite(pwmPinA, -abs(totoz -1.1* euler.z()) /2); // asservissement de la vitesse du moteur avec un gyromètre
165  }
166  else {
167//    totoz = 3;
168    digitalWrite(dirPinA, HIGH);
169    analogWrite(pwmPinA, -abs(totoz -1.1* euler.z()) /2); // asservissement de la vitesse du moteur avec un gyromètre
170  }
171  if(/*totoy */-1.1* euler.y() < 0) {
172//    totoy = 12;
173    digitalWrite(dirPinB, LOW);
174    analogWrite(pwmPinB, -abs(totoy -1.1* euler.y()) /2); // asservissement de la vitesse du moteur avec un gyromètre
175  }
176  else {
177//    totoy = 12;
178    digitalWrite(dirPinB, HIGH);
179    analogWrite(pwmPinB, -abs(totoy -1.1* euler.y()) /2); // asservissement de la vitesse du moteur avec un gyromètre
180  }
181  //analogWrite(pwmPin, 100);
182}