1 changed files with 0 additions and 407 deletions
@ -1,407 +0,0 @@ |
|||
#include <Wire.h> |
|||
#include <TimerOne.h> |
|||
#include <SoftwareSerial.h> |
|||
#include <ArduinoJson.h> |
|||
|
|||
#define MPU9250_ADDRESS 0x68 |
|||
#define MAG_ADDRESS 0x0C |
|||
|
|||
#define GYRO_FULL_SCALE_250_DPS 0x00 |
|||
#define GYRO_FULL_SCALE_500_DPS 0x08 |
|||
#define GYRO_FULL_SCALE_1000_DPS 0x10 |
|||
#define GYRO_FULL_SCALE_2000_DPS 0x18 |
|||
|
|||
#define ACC_FULL_SCALE_2_G 0x00 |
|||
#define ACC_FULL_SCALE_4_G 0x08 |
|||
#define ACC_FULL_SCALE_8_G 0x10 |
|||
#define ACC_FULL_SCALE_16_G 0x18 |
|||
|
|||
// This function read Nbytes bytes from I2C device at address Address.
|
|||
// Put read bytes starting at register Register in the Data array.
|
|||
void I2Cread(uint8_t Address, uint8_t Register, uint8_t Nbytes, uint8_t* Data) |
|||
{ |
|||
// Set register address
|
|||
Wire.beginTransmission(Address); |
|||
Wire.write(Register); |
|||
Wire.endTransmission(); |
|||
|
|||
// Read Nbytes
|
|||
Wire.requestFrom(Address, Nbytes); |
|||
uint8_t index=0; |
|||
while (Wire.available()) |
|||
Data[index++]=Wire.read(); |
|||
} |
|||
StaticJsonBuffer<200> jsonBuffer; |
|||
|
|||
// Build your own object tree in memory to store the data you want to send in the request
|
|||
JsonObject& root = jsonBuffer.createObject(); |
|||
|
|||
SoftwareSerial ArduinoMaster(2,3); |
|||
String msg; |
|||
// Write a byte (Data) in device (Address) at register (Register)
|
|||
void I2CwriteByte(uint8_t Address, uint8_t Register, uint8_t Data) |
|||
{ |
|||
// Set register address
|
|||
Wire.beginTransmission(Address); |
|||
Wire.write(Register); |
|||
Wire.write(Data); |
|||
Wire.endTransmission(); |
|||
} |
|||
|
|||
// Initial time
|
|||
long int ti; |
|||
volatile bool intFlag=false; |
|||
int enableA = 11; |
|||
int pinA1 = 6; |
|||
int pinA2 = 5; |
|||
int enableB = 10; |
|||
int pinB1 = 4; |
|||
int pinB2 = 3; |
|||
boolean run = true; |
|||
|
|||
// Initializations
|
|||
void setup() |
|||
{ |
|||
pinMode(enableA, OUTPUT); |
|||
pinMode(pinA1, OUTPUT); |
|||
pinMode(pinA2, OUTPUT); |
|||
|
|||
pinMode(enableB, OUTPUT); |
|||
pinMode(pinB1, OUTPUT); |
|||
pinMode(pinB2, OUTPUT); |
|||
// Arduino initializations
|
|||
pinMode(13,OUTPUT); |
|||
Wire.begin(); |
|||
ArduinoMaster.begin(9600); |
|||
Serial.begin(9600); |
|||
|
|||
// Set accelerometers low pass filter at 5Hz
|
|||
I2CwriteByte(MPU9250_ADDRESS,29,0x06); |
|||
// Set gyroscope low pass filter at 5Hz
|
|||
I2CwriteByte(MPU9250_ADDRESS,26,0x06); |
|||
|
|||
|
|||
// Configure gyroscope range
|
|||
I2CwriteByte(MPU9250_ADDRESS,27,GYRO_FULL_SCALE_1000_DPS); |
|||
// Configure accelerometers range
|
|||
I2CwriteByte(MPU9250_ADDRESS,28,ACC_FULL_SCALE_4_G); |
|||
// Set by pass mode for the magnetometers
|
|||
I2CwriteByte(MPU9250_ADDRESS,0x37,0x02); |
|||
|
|||
// Request continuous magnetometer measurements in 16 bits
|
|||
I2CwriteByte(MAG_ADDRESS,0x0A,0x16); |
|||
|
|||
pinMode(13, OUTPUT); |
|||
Timer1.initialize(10000); // initialize timer1, and set a 1/2 second period
|
|||
Timer1.attachInterrupt(callback); // attaches callback() as a timer overflow interrupt
|
|||
|
|||
|
|||
// Store initial time
|
|||
ti=millis(); |
|||
} |
|||
|
|||
// Counter
|
|||
long int cpt=0; |
|||
|
|||
void callback() |
|||
{ |
|||
intFlag=true; |
|||
digitalWrite(13, digitalRead(13) ^ 1); |
|||
} |
|||
|
|||
// Main loop, read and display data
|
|||
void loop() |
|||
{ |
|||
readSerialPort(); |
|||
// Send answer to master
|
|||
if(msg!=""){ |
|||
Serial.print("Master sent : " ); |
|||
Serial.println(msg); |
|||
ArduinoMaster.print(msg); |
|||
msg=""; |
|||
} |
|||
while (!intFlag); |
|||
intFlag=false; |
|||
|
|||
// Display time
|
|||
Serial.print (millis()-ti,DEC); |
|||
Serial.print ("\t"); |
|||
|
|||
|
|||
// _______________
|
|||
// ::: Counter :::
|
|||
|
|||
// Display data counter
|
|||
// Serial.print (cpt++,DEC);
|
|||
// Serial.print ("\t");
|
|||
|
|||
|
|||
|
|||
// ____________________________________
|
|||
// ::: accelerometer and gyroscope :::
|
|||
|
|||
// Read accelerometer and gyroscope
|
|||
uint8_t Buf[14]; |
|||
I2Cread(MPU9250_ADDRESS,0x3B,14,Buf); |
|||
|
|||
// Create 16 bits values from 8 bits data
|
|||
|
|||
// Accelerometer
|
|||
int16_t ax=-(Buf[0]<<8 | Buf[1]); |
|||
int16_t ay=-(Buf[2]<<8 | Buf[3]); |
|||
int16_t az=Buf[4]<<8 | Buf[5]; |
|||
|
|||
// Gyroscope
|
|||
int16_t gx=-(Buf[8]<<8 | Buf[9]); |
|||
int16_t gy=-(Buf[10]<<8 | Buf[11]); |
|||
int16_t gz=Buf[12]<<8 | Buf[13]; |
|||
|
|||
root["acceleratorx"]= ax; |
|||
root["acceleratory"]= ay; |
|||
root["acceleratorz"]= az; |
|||
|
|||
root["gyroscopex"]= ax; |
|||
root["gyroscopey"]= ay; |
|||
root["gyroscopez"]= az; |
|||
root.prettyPrintTo(Serial); |
|||
Serial.println(); |
|||
|
|||
// Display values
|
|||
// Accelerometer
|
|||
|
|||
/*
|
|||
Serial.print ("ax: "); |
|||
Serial.print (ax,DEC); |
|||
Serial.print ("\t"); |
|||
Serial.print ("ay :"); |
|||
Serial.print (ay,DEC); |
|||
Serial.print ("\t"); |
|||
Serial.print ("az :"); |
|||
Serial.print (az,DEC); |
|||
Serial.print ("\t"); |
|||
*/ |
|||
// Gyroscope
|
|||
|
|||
/*
|
|||
Serial.print ("gx: "); |
|||
Serial.print (gx,DEC); |
|||
Serial.print ("\t"); |
|||
Serial.print ("gy: "); |
|||
Serial.print (gy,DEC); |
|||
Serial.print ("\t"); |
|||
Serial.print ("gz: "); |
|||
Serial.print (gz,DEC); |
|||
Serial.print ("\t"); |
|||
*/ |
|||
/*
|
|||
// _____________________
|
|||
// ::: Magnetometer :::
|
|||
|
|||
|
|||
// Read register Status 1 and wait for the DRDY: Data Ready
|
|||
|
|||
uint8_t ST1; |
|||
do |
|||
{ |
|||
I2Cread(MAG_ADDRESS,0x02,1,&ST1); |
|||
} |
|||
while (!(ST1&0x01)); |
|||
|
|||
// Read magnetometer data
|
|||
uint8_t Mag[7]; |
|||
I2Cread(MAG_ADDRESS,0x03,7,Mag); |
|||
|
|||
// Create 16 bits values from 8 bits data
|
|||
|
|||
// Magnetometer
|
|||
int16_t mx=-(Mag[3]<<8 | Mag[2]); |
|||
int16_t my=-(Mag[1]<<8 | Mag[0]); |
|||
int16_t mz=-(Mag[5]<<8 | Mag[4]); |
|||
|
|||
|
|||
// Magnetometer
|
|||
Serial.print (mx+200,DEC); |
|||
Serial.print ("\t"); |
|||
Serial.print (my-70,DEC); |
|||
Serial.print ("\t"); |
|||
Serial.print (mz-700,DEC); |
|||
Serial.print ("\t"); |
|||
|
|||
|
|||
|
|||
// End of line*/
|
|||
Serial.println(""); |
|||
// delay(500000);
|
|||
|
|||
} |
|||
|
|||
|
|||
void readSerialPort(){ |
|||
while (ArduinoMaster.available()) { |
|||
delay(10); |
|||
if (ArduinoMaster.available() >0) { |
|||
char c = ArduinoMaster.read(); //gets one byte from serial buffer
|
|||
msg += c; //makes the string readString
|
|||
} |
|||
} |
|||
ArduinoMaster.flush(); |
|||
} |
|||
void enableMotors() |
|||
{ |
|||
motorAOn(); |
|||
motorBOn(); |
|||
} |
|||
|
|||
void disableMotors() |
|||
{ |
|||
motorAOff(); |
|||
motorBOff(); |
|||
} |
|||
|
|||
void forward(int time) |
|||
{ |
|||
motorAForward(); |
|||
motorBForward(); |
|||
delay(time); |
|||
} |
|||
|
|||
void backward(int time) |
|||
{ |
|||
motorABackward(); |
|||
motorBBackward(); |
|||
delay(time); |
|||
} |
|||
|
|||
void turnLeft(int time) |
|||
{ |
|||
motorABackward(); |
|||
motorBForward(); |
|||
delay(time); |
|||
} |
|||
|
|||
void turnRight(int time) |
|||
{ |
|||
motorAForward(); |
|||
motorBBackward(); |
|||
delay(time); |
|||
} |
|||
|
|||
void coast(int time) |
|||
{ |
|||
motorACoast(); |
|||
motorBCoast(); |
|||
delay(time); |
|||
} |
|||
|
|||
void brake(int time) |
|||
{ |
|||
motorABrake(); |
|||
motorBBrake(); |
|||
delay(time); |
|||
} |
|||
//Define low-level H-bridge commands
|
|||
|
|||
//enable motors
|
|||
void motorAOn() |
|||
{ |
|||
digitalWrite(enableA, HIGH); |
|||
} |
|||
|
|||
void motorBOn() |
|||
{ |
|||
digitalWrite(enableB, HIGH); |
|||
} |
|||
|
|||
//disable motors
|
|||
void motorAOff() |
|||
{ |
|||
digitalWrite(enableB, LOW); |
|||
} |
|||
|
|||
void motorBOff() |
|||
{ |
|||
digitalWrite(enableA, LOW); |
|||
} |
|||
|
|||
//motor A controls
|
|||
void motorAForward() |
|||
{ |
|||
digitalWrite(pinA1, HIGH); |
|||
digitalWrite(pinA2, LOW); |
|||
} |
|||
|
|||
void motorABackward() |
|||
{ |
|||
digitalWrite(pinA1, LOW); |
|||
digitalWrite(pinA2, HIGH); |
|||
} |
|||
|
|||
//motor B controls
|
|||
void motorBForward() |
|||
{ |
|||
digitalWrite(pinB1, HIGH); |
|||
digitalWrite(pinB2, LOW); |
|||
} |
|||
|
|||
void motorBBackward() |
|||
{ |
|||
digitalWrite(pinB1, LOW); |
|||
digitalWrite(pinB2, HIGH); |
|||
} |
|||
|
|||
//coasting and braking
|
|||
void motorACoast() |
|||
{ |
|||
digitalWrite(pinA1, LOW); |
|||
digitalWrite(pinA2, LOW); |
|||
} |
|||
|
|||
void motorABrake() |
|||
{ |
|||
digitalWrite(pinA1, HIGH); |
|||
digitalWrite(pinA2, HIGH); |
|||
} |
|||
|
|||
void motorBCoast() |
|||
{ |
|||
digitalWrite(pinB1, LOW); |
|||
digitalWrite(pinB2, LOW); |
|||
} |
|||
|
|||
void motorBBrake() |
|||
{ |
|||
digitalWrite(pinB1, HIGH); |
|||
digitalWrite(pinB2, HIGH); |
|||
} |
|||
/*bool sendRequest(const char* host, const char* resource) {
|
|||
// Reserve memory space for your JSON data
|
|||
StaticJsonBuffer<200> jsonBuffer; |
|||
|
|||
// Build your own object tree in memory to store the data you want to send in the request
|
|||
JsonObject& root = jsonBuffer.createObject(); |
|||
root["sensor"] = "dht11"; |
|||
|
|||
JsonObject& data = root.createNestedObject("data"); |
|||
data.set("temperature", "30.1"); |
|||
data.set("humidity", "70.1"); |
|||
|
|||
// Generate the JSON string
|
|||
root.printTo(Serial); |
|||
|
|||
Serial.print("POST "); |
|||
Serial.println(resource); |
|||
|
|||
client.print("POST "); |
|||
client.print(resource); |
|||
client.println(" HTTP/1.1"); |
|||
client.print("Host: "); |
|||
client.println(host); |
|||
client.println("Connection: close\r\nContent-Type: application/json"); |
|||
client.print("Content-Length: "); |
|||
client.print(root.measureLength()); |
|||
client.print("\r\n"); |
|||
client.println(); |
|||
root.printTo(client); |
|||
|
|||
return true; |
|||
}*/ |
|||
Loading…
Reference in new issue