* Telemetry and Custom Command Sample Arduino Code

*   Telemetry Example: Monitor an extended I/O register value and send to control panel display

*   Command Example:   Add Robot unique MOVE, BLINK, and SERVO commands

*/

#include <Robot3DoTBoard.h>     // instantiated as Robot3DoT at end of class header

#include <EEPROM.h>

//#include <Wire.h>               // I2C support

#include <Servo.h>

#include <I2C.h>

// IMU Variables

#include “gyro_accel.h”

// Defining constants

#define IMUdt 20                       // time difference in milli seconds

#define rad2degree 57.3              // Radian to degree conversion

#define Filter_gain 0.95             // e.g.  angle = angle_gyro*Filter_gain + angle_accel*(1-Filter_gain)

// *********************************************************************

//    Global IMU Variables

// *********************************************************************

unsigned long IMUt=0; // Time Variables

float angle_x_gyro=0,angle_y_gyro=0,angle_z_gyro=0,angle_x_accel=0,angle_y_accel=0,angle_z_accel=0,angle_x=0,angle_y=0,angle_z=0;

// *********************************************************************

// Motor Variables

int Motor1F = 5;

int Motor1R = 10;

int Motor1PWM = A9;

volatile int Motor1Speed = 0;

int Motor2F = 19;

int Motor2R = 20;

int Motor2PWM = 6;

volatile int Motor2Speed = 0;

volatile int Mot1State = 0;

volatile int Mot2State = 0;

long HoldTime = micros();

// Servo Variables

//

Robot3DoTBoard Robot3DoT;       // instantiated as Robot3DoT at end of class header

/*

* Command Example

* Step 1: Assign new command mnemonics ID numbers

*         In our example we will be adding 3 custom commands (2 new and one predefined).

*         The predefined MOVE command is intercepted and our robot unique code is to be

*         run instead. The MOVE command mnemonic ID 0x01 is already defined in Configure.h

*         The next two commands are new and assigned to the first two addresses in the

*         custom command address space 0x40 – 0x5F.

*/

#define DYNSTAT       0x41

const uint8_t CMD_LIST_SIZE = 2;   // we are adding 3 commands (MOVE, BLINK, SERVO)

/*

* Command Example

* Step 2: Register commands by linking IDs to their corresponding command handlers

*         In our example when the MOVE command is intercepted the moveHandler is to be run.

*         In a similar fashion the BLINK command calls the blinkHandler and SERVO the

*         servoHandler.

*/

void moveHandler (uint8_t cmd, uint8_t param[], uint8_t n);

void dynstatHandler (uint8_t cmd, uint8_t param[], uint8_t n);

Robot3DoTBoard::cmdFunc_t onCommand[CMD_LIST_SIZE] = {{MOVE,moveHandler}, {DYNSTAT,dynstatHandler}};

/*

* Telemetry Example

* Step 1: Instantiate packet

*         In our example we simulate a current sensor wired to MOTOR 2. MOTOR2_CURRENT_ID

*         is defined as 0x02 in Configure.h

*         To simulate the data stream coming from the sensor we will read ATmega32U4

*         Register OCR4D which controls the duty cycle of MOTOR 2.

*/

Packet motorPWM(MOTOR2_CURRENT_ID);  // initialize the packet properties to default values

void setup()

{

Serial.begin(38400);               // default = 115200

Robot3DoT.begin();

// Initialize Motor To Off

pinMode(Motor1F, OUTPUT);

pinMode(Motor1R, OUTPUT);

pinMode(Motor1PWM, OUTPUT);

pinMode(Motor2F, OUTPUT);

pinMode(Motor2R, OUTPUT);

pinMode(Motor2PWM, OUTPUT);

digitalWrite(Motor1F,LOW);

digitalWrite(Motor1R,LOW);

digitalWrite(Motor2F,LOW);

digitalWrite(Motor2R,LOW);

// Initialize IMU

Serial.begin(9600);

I2c.begin();

I2c.timeOut(100);

MPU6050_ResetWake();

MPU6050_SetGains(0,1);// Setting the lows scale

MPU6050_SetDLPF(0); // Setting the DLPF to inf Bandwidth for calibration

MPU6050_OffsetCal();

MPU6050_SetDLPF(6); // Setting the DLPF to lowest Bandwidth

IMUt=millis();

/*

* Command Example

* Step 3: Tell 3DoT Robot software about new commands

*

*

*/

Robot3DoT.setOnCommand(onCommand, CMD_LIST_SIZE);

/* Telemetry Example

* Step 2: Modify default values assigned to internal properties as needed.

*         Before a packet is created and sent, it is qualified. Specifically,

*         the data in a packet must change by some amount from the previous

*         packet and may not be sent with at a period less than some value.

*         In most cases you can leave these values at their default values.

*/

delay(1000);

}

void loop()

{

Robot3DoT.loop();

// Set Motor State to the Telemetry Read Value

if (Mot1State == 1){ digitalWrite(Motor1F,HIGH);

digitalWrite(Motor1R,LOW);

} else if (Mot1State == 2) { digitalWrite(Motor1F,LOW);

digitalWrite(Motor1R,HIGH);

}                                 else {digitalWrite(Motor1F,LOW);

digitalWrite(Motor1R,LOW);}

if (Mot2State == 1){  digitalWrite(Motor2F,HIGH);

digitalWrite(Motor2R,LOW);

} else if (Mot2State == 2) { digitalWrite(Motor2F,LOW);

digitalWrite(Motor2R,HIGH);

}                                 else {digitalWrite(Motor2F,LOW);

digitalWrite(Motor2R,LOW);}

analogWrite(Motor1PWM, Motor1Speed);

analogWrite(Motor2PWM, Motor2Speed);

if (micros() – HoldTime > 500000){

HoldTime = micros();

//  Serial.write(“”);

//  Serial.write(Mot1State);

//  Serial.write(Motor1Speed);

//  Serial.write(Mot2State);

//  Serial.write(Motor2Speed);

//  Serial.write(“”);

Serial.print(angle_x);

Serial.print(“\t”);

Serial.print(angle_y);

Serial.print(“\t”);

Serial.print(angle_z);

Serial.println();

}

// Read IMU Settings

//if (millis() – IMUt > IMUdt){

IMU();

//}

// Set Servos to IMU Setting

}

/*

* Command Example

* Step 4: Write command handlers

*/

/*

* User Defined Command BLINK (0x40) Example

* A5 01 40 E4

*/

/*

* Override MOVE (0x01) Command Example

* A5 05 01 01 80 01 80 A1

*/

void moveHandler (uint8_t cmd, uint8_t param[], uint8_t n)

{

Mot1State = param[0];

Mot2State = param[2];

// Configure Inputs and Output

Motor1Speed = param[1];

Motor2Speed = param[3];

Serial.write(cmd);             // move command = 0x01

Serial.write(n);               // number of param = 4

for (int i=0;i<n;i++)          // param = 01 80 01 80

{

Serial.write (param[i]);

}

}                                // moveHandler

/*

* User Defined Command SERVO (0x41) Example

* Rotate servo to 90 degrees

* A5 02 41 90 76

*/

void dynstatHandler (uint8_t cmd, uint8_t param[], uint8_t n)

{

Serial.write(cmd);             // servo command = 0x41

Serial.write(n);               // number of param = 1

for (int i=0;i<n;i++)          // param = 90 degrees

{

Serial.write (param[i]);

}

}  // servoHandler

void IMU(){

IMUt=millis();

MPU6050_ReadData();

angle_x_gyro = (gyro_x_scalled*((float)IMUdt/1000)+angle_x);

angle_y_gyro = (gyro_y_scalled*((float)IMUdt/1000)+angle_y);

angle_z_gyro = (gyro_z_scalled*((float)IMUdt/1000)+angle_z);

angle_z_accel = atan(accel_z_scalled/(sqrt(accel_y_scalled*accel_y_scalled+accel_x_scalled*accel_x_scalled)))*(float)rad2degree;

angle_y_accel = -atan(accel_y_scalled/(sqrt(accel_y_scalled*accel_y_scalled+accel_z_scalled*accel_z_scalled)))*(float)rad2degree;

angle_x_accel = atan(accel_x_scalled/(sqrt(accel_x_scalled*accel_x_scalled+accel_z_scalled*accel_z_scalled)))*(float)rad2degree;

angle_x = Filter_gain*angle_x_gyro+(1-Filter_gain)*angle_x_accel;

angle_y = Filter_gain*angle_y_gyro+(1-Filter_gain)*angle_y_accel;

angle_z = Filter_gain*angle_z_gyro+(1-Filter_gain)*angle_z_accel;

//  Serial.print(gyro_x_scalled);

//  Serial.print(“\t”);

//  Serial.print(gyro_y_scalled);

//  Serial.print(“\t”);

//  Serial.print(gyro_z_scalled);

//  Serial.print(“\t”);

//

//

//  Serial.print(accel_x_scalled);

//  Serial.print(“\t”);

//  Serial.print(accel_y_scalled);

//  Serial.print(“\t”);

//  Serial.print(accel_z_scalled);

//  Serial.print(“\t”);

//

//  Serial.print(angle_x_gyro);

//  Serial.print(“\t”);

//  Serial.print(angle_y_gyro);

//  Serial.print(“\t”);

//  Serial.print(angle_z_gyro);

//  Serial.print(“\t”);

//

//  Serial.print(angle_x_accel);

//  Serial.print(“\t”);

//  Serial.print(angle_y_accel);

//  Serial.print(“\t”);

//  Serial.print(angle_z_accel);

//  Serial.print(“\t”);

//  Serial.print(angle_x);

//  Serial.print(“\t”);

//  Serial.print(angle_y);

//  Serial.print(“\t”);

//  Serial.print(angle_z);

//  Serial.println();

//  Serial.println(((float)(millis()-IMUt)/(float)IMUdt)*100);

}

Last edit date = 2014-10-08

Website: http://hobbylogs.me.pn/?p=47

Location: Pakistan

Ver: 0.1 beta — Start

Ver: 0.2 beta — Bug fixed for calculating “angle_y_accel” and “angle_x_accel” in “Gyro_header.ino” file

*/

#include <Arduino.h>

#include <Wire.h>

#include “gyro_accel.h”

#include “I2C.h”

// Defining the important registers

#define MPU6050_address 0x68

// —— SELF Test Trim Factors ———————-

#define MPU6050_self_test_x 13       // R/W

#define MPU6050_self_test_y 14       // R/W

#define MPU6050_self_test_z 15       // R/W

#define MPU6050_self_test_A 16       // R/W

// —– Sample Divider ——————————-

#define MPU6050_sample_div 25        // R/W

/*       Sample Divider Discription

Sample rate = 8/(1 + Sample rate divider) [kHz]  if DLPF is disabled

Sample rate = 1/(1 + Sample rate divider) [kHz]  if DLPF is enabled

*/

// —– Configration ———————————

#define MPU6050_config 26            // R/W

#define MPU6050_gyro_config 27       // R/W

#define MPU6050_accel_config 28      // R/W

// —– Data ———————————————

#define MPU6050_data_start 59

#define read_bytes 14

// —– Power Management ———————————

#define MPU6050_PWR1 107

#define MPU6050_PWR2 108

// —– Defining Constant ——————————–

#define g 9.81                       // Gravitational accelration

// Variables

int temp=0, accel_x=0, accel_y=0, accel_z=0, gyro_x=0, gyro_y=0, gyro_z=0; // Raw values varaibles

int accel_x_OC=0, accel_y_OC=0, accel_z_OC=0, gyro_x_OC=0 ,gyro_y_OC=0, gyro_z_OC=0; // offset variables

float temp_scalled,accel_x_scalled,accel_y_scalled,accel_z_scalled,gyro_x_scalled,gyro_y_scalled,gyro_z_scalled; //Scalled Data varaibles

float accel_scale_fact = 1, gyro_scale_fact = 1; // Scale factor variables

// **************************************************************************

//          Functions for MPU6050

// **************************************************************************

void MPU6050_ReadData(){

//Wire.beginTransmission(MPU6050_address);

//Wire.write(MPU6050_data_start);

//Wire.endTransmission();

I2c.write(MPU6050_address, MPU6050_data_start);

//int read_bytes = 14;

//Wire.requestFrom(MPU6050_address,read_bytes);

I2c.read(MPU6050_address,read_bytes);

if(I2c.available() == read_bytes){

accel_x = I2c.receive()<<8 | I2c.receive();

accel_y = I2c.receive()<<8 | I2c.receive();

accel_z = I2c.receive()<<8 | I2c.receive();

temp = I2c.receive()<<8 | I2c.receive();

gyro_x = I2c.receive()<<8 | I2c.receive();

gyro_y = I2c.receive()<<8 | I2c.receive();

gyro_z = I2c.receive()<<8 | I2c.receive();

}

/*   if(Wire.available() == read_bytes){

accel_x = Wire.read()<<8 | Wire.read();

accel_y = Wire.read()<<8 | Wire.read();

accel_z = Wire.read()<<8 | Wire.read();

temp = Wire.read()<<8 | Wire.read();

gyro_x = Wire.read()<<8 | Wire.read();

gyro_y = Wire.read()<<8 | Wire.read();

gyro_z = Wire.read()<<8 | Wire.read();

} */

accel_x_scalled = (float)(accel_x-accel_x_OC)*accel_scale_fact/1000; // divided by 1000 as the Scale factor is in milli units

accel_y_scalled = (float)(accel_y-accel_y_OC)*accel_scale_fact/1000;

accel_z_scalled = (float)(accel_z-accel_z_OC)*accel_scale_fact/1000;

gyro_x_scalled = (float)(gyro_x-gyro_x_OC)*gyro_scale_fact/1000;

gyro_y_scalled = (float)(gyro_y-gyro_y_OC)*gyro_scale_fact/1000;

gyro_z_scalled = ((float)(gyro_z-gyro_z_OC)*gyro_scale_fact/1000);

temp_scalled = (float)temp/340+36.53;

}

// ————————————————

//       Reset and wake up the MPU6050

// ————————————————

void MPU6050_ResetWake(){

Serial.println(“Resetting MPU6050 and waking it up…..”);

Wire.beginTransmission(MPU6050_address);

Wire.write(MPU6050_PWR1);

Wire.write(0b10000000);

Wire.endTransmission();

delay(100); // Waiting for the reset to complete

Wire.beginTransmission(MPU6050_address);

Wire.write(MPU6050_PWR1);

Wire.write(0b00000000);

Wire.endTransmission();

}

// ————————————————

//       Setting up the DLFP

// ————————————————

void MPU6050_SetDLPF(int BW)

{

if (BW < 0 || BW > 6){

BW = 0;

}

Wire.beginTransmission(MPU6050_address);

Wire.write(MPU6050_config); // Address to the configuration register

/*       config Discription —- x x 0 0 0 F2 F1 F0

I am only intrested in the Digital Low Pass Filter (DLPF)

F2 F1 F0    Bandwidth [Hz]

0  0  0

0  0  1      184

0  1  0      94

0  1  1      44

1  0  0      21

1  0  1      10

1  1  0      5

*/

Wire.write(BW);

Wire.endTransmission();

}

// ————————————————

//       Setting up the Accelrometer and Gyro Gains

// ————————————————

void MPU6050_SetGains(int gyro,int accel)

{

byte gyro_byte,accel_byte;

// Setting up Gyro

Wire.beginTransmission(MPU6050_address);

Wire.write(MPU6050_gyro_config); // Address to the configuration register

if (gyro==0)

{

gyro_scale_fact =(float)250*0.0305; // each data is of 16 bits that means, 250 is divided along 2^(15)-1 = 32767 so for milli degree/s 0.0305 = 1000/32767

gyro_byte = 0b00000000;

}else if (gyro == 1)

{

gyro_scale_fact = 500*0.0305; // each data is of 16 bits that means, 500 is divided along 2^(15)-1 = 32767 so for milli degree/s 0.0305 = 1000/32767

gyro_byte = 0b00001000;

}else if (gyro == 2)

{

gyro_scale_fact = 1000*0.0305;// each data is of 16 bits that means, 1000 is divided along 2^(15)-1 = 32767 so for milli degree/s 0.0305 = 1000/32767

gyro_byte = 0b00010000;

}else if (gyro == 3)

{

gyro_scale_fact = 2000*0.0305;  // each data is of 16 bits that means, 2000 is divided along 2^(15)-1 = 32767 so for milli degree/s 0.0305 = 1000/32767

gyro_byte = 0b00011000;

}else

{

gyro_scale_fact = 1;

}

Wire.write(gyro_byte);

Wire.endTransmission();

Serial.print(“The gyro scale is set to “);

Serial.print(gyro_scale_fact);

Serial.println(” milli Degree/s”);

// Setting up Accel

Wire.beginTransmission(MPU6050_address);

Wire.write(MPU6050_accel_config); // Address to the configuration register

if (accel==0)

{

accel_scale_fact =(float)2*g*0.0305; // each data is of 16 bits that means, 2g is divided along 2^(15)-1 = 32767 so for milli m/s^2 0.0305 = 1000/32767

accel_byte = 0b00000000;

}else if (accel == 1)

{

accel_scale_fact = 4*g*0.0305; // each data is of 16 bits that means, 4g is divided along 2^(15)-1 = 32767 so for milli m/s^2 0.0305 = 1000/32767

accel_byte = 0b00001000;

}else if (accel == 2)

{

accel_scale_fact = 8*g*0.0305;// each data is of 16 bits that means, 8g is divided along 2^(15)-1 = 32767 so for milli m/s^2 0.0305 = 1000/32767

accel_byte = 0b00010000;

}else if (accel == 3)

{

accel_scale_fact = 16*g*0.0305; // each data is of 16 bits that means, 16g is divided along 2^(15)-1 = 32767 so for milli m/s^2 0.0305 = 1000/32767

accel_byte = 0b00011000;

}else

{

accel_scale_fact = 1;

}

Wire.write(accel_byte);

Wire.endTransmission();

Serial.print(“The accel scale is set to “);

Serial.print(accel_scale_fact);

Serial.println(” milli m/s^2″);

}

// ————————————————

//       offset calibration

// ————————————————

void MPU6050_OffsetCal(){

Serial.println(“Calibrating gyroscope …. dont move the hardware ……….”);

int x=0,y=0,z=0,i;

MPU6050_ReadData();

MPU6050_ReadData();

// Gyro Offset Calculation

x=gyro_x;

y=gyro_y;

z=gyro_z;

for (i=1;i<=1000;i++){

MPU6050_ReadData();

x=(x+gyro_x)/2;

y=(y+gyro_y)/2;

z=(z+gyro_z)/2;

Serial.print(“.”);

}

Serial.println(“.”);

gyro_x_OC=x;

gyro_y_OC=y;

gyro_z_OC=z;

Serial.print(“gyro_x register offset = “);

Serial.println(x);

Serial.print(“gyro_y register offect = “);

Serial.println(y);

Serial.print(“gyro_z register offset = “);

Serial.println(z);

// Accel Offset Calculation

Serial.println(“Calibrating accelrometer …. dont move the hardware ……….”);

x=accel_x;

y=accel_y;

z=accel_z;

for (i=1;i<=1000;i++){

MPU6050_ReadData();

x=(x+accel_x)/2;

y=(y+accel_y)/2;

z=(z+accel_z)/2;

Serial.print(“.”);

}

Serial.println(“.”);

accel_x_OC=x;

accel_y_OC=y;

accel_z_OC=z-(float)g*1000/accel_scale_fact;

Serial.print(“Accel_x register offset = “);

Serial.println(x);

Serial.print(“Accel_y register offect = “);

Serial.println(y);

Serial.print(“Accel_z register offset = “);

Serial.println(z);

}