diff --git a/pROJECT.ino b/pROJECT.ino new file mode 100644 index 0000000..b615846 --- /dev/null +++ b/pROJECT.ino @@ -0,0 +1,407 @@ +#include +#include +#include +#include + +#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; +}*/