tried to post the full code but the forum spit it back saying it was over the max 9500 chars. here is the TX so ill have to split it in 2 or more posts
Part_01
#include <NewPing.h>
#include <Arduino.h>
#include <Wire.h>
#include <stdio.h>
#include <Mirf.h>
#include <MirfHardwareSpiDriver.h>
#include <MirfSpiDriver.h>
#include <nRF24L01.h>
#include <SPI.h>
#include <TinyGPS.h>
//-+-+-+>>>> START Ultrasonic Sensor section<<<<<<-+-+-+
#define TRIGGER_PIN 23 // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN 22 // Arduino pin tied to echo pin on the ultrasonic sensor.
#define MAX_DISTANCE 200 // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.
//-+-+-+>>>> END Ultrasonic Sensor section<<<<<<-+-+-+
//-+-+-+>>>> START MotorDriver Vars section<<<<<<-+-+-+
#define Mspeed0 0
#define Mspeed1 100
#define Mspeed2 150
#define runs1 2
#define runs2 3
#define mDelay1 100
#define mDelay2 150
//-+-+-+>>>> END MotorDriver Vars section<<<<<<-+-+-+
//-+-+-+>>>> START GPS Vars section<<<<<<-+-+-+
#define WPT_IN_RANGE_M 1
//-+-+-+>>>> END GPS Vars section<<<<<<-+-+-+
//-+-+-+>>>> SRTART POLOLU MINIMU - 9DOF Sensor section<<<<<<-+-+-+
// LSM303 accelerometer: 8 g sensitivity
// 3.8 mg/digit; 1 g = 256
#define GRAVITY 256 //this equivalent to 1G in the raw data coming from the accelerometer
#define ToRad(x) ((x)*0.01745329252) // *pi/180
#define ToDeg(x) ((x)*57.2957795131) // *180/pi
// L3G4200D gyro: 2000 dps full scale
// 70 mdps/digit; 1 dps = 0.07
#define Gyro_Gain_X 0.07 //X axis Gyro gain
#define Gyro_Gain_Y 0.07 //Y axis Gyro gain
#define Gyro_Gain_Z 0.07 //Z axis Gyro gain
#define Gyro_Scaled_X(x) ((x)*ToRad(Gyro_Gain_X)) //Return the scaled ADC raw data of the gyro in radians for second
#define Gyro_Scaled_Y(x) ((x)*ToRad(Gyro_Gain_Y)) //Return the scaled ADC raw data of the gyro in radians for second
#define Gyro_Scaled_Z(x) ((x)*ToRad(Gyro_Gain_Z)) //Return the scaled ADC raw data of the gyro in radians for second
// LSM303 magnetometer calibration constants; use the Calibrate example from
// the Pololu LSM303 library to find the right values for your board
#define M_X_MIN -602
#define M_Y_MIN -668
#define M_Z_MIN -592
#define M_X_MAX 391
#define M_Y_MAX 422
#define M_Z_MAX 524
#define Kp_ROLLPITCH 0.02
#define Ki_ROLLPITCH 0.00002
#define Kp_YAW 1.2
#define Ki_YAW 0.00002
//OUTPUTMODE=1 will print the corrected data,
//OUTPUTMODE=0 will print uncorrected data of the gyros (with drift)
#define OUTPUTMODE 1
//#define PRINT_DCM 0 //Will print the whole direction cosine matrix
#define PRINT_ANALOGS 0 //Will print the analog raw data
#define PRINT_EULER 1 //Will print the Euler angles Roll, Pitch and Yaw
#define STATUS_LED 13
//-+-+-+>>>> END POLOLU MINIMU - 9DOF Sensor section<<<<<<-+-+-+
//-+-+-+>>>> START MotorDriver Vars section<<<<<<-+-+-+
int DIR_A = 12; //L298 DIR A Digital High/Low = left/right
int PWM_A = 3; //L298 PWM A Analog
int ENA_A = 9; //L298 Enable A Digital Hight/low on/off
int DIR_B = 13; //L298 DIR A Digital High/Low = left/right
int PWM_B = 11; //L298 PWM A Analog
int ENA_B = 8; //L298 Enable A Digital Hight/low on/off
int oTime = 0;
//-+-+-+>>>> END MotorDriver Vars section<<<<<<-+-+-+
//-+-+-+>>>> SRTART POLOLU MINIMU - 9DOF Sensor section<<<<<<-+-+-+
int SENSOR_SIGN[9] = {
1,1,1,-1,-1,-1,1,1,1}; //Correct directions x,y,z - gyro, accelerometer, magnetometer
float G_Dt=0.02; // Integration time (DCM algorithm) We will run the integration loop at 50Hz if possible
long timer=0; //general purpuse timer
long timer_old;
long timer24=0; //Second timer used to print values
int AN[6]; //array that stores the gyro and accelerometer data
int AN_OFFSET[6]={
0,0,0,0,0,0}; //Array that stores the Offset of the sensors
int gyro_x;
int gyro_y;
int gyro_z;
int accel_x;
int accel_y;
int accel_z;
int magnetom_x;
int magnetom_y;
int magnetom_z;
float c_magnetom_x;
float c_magnetom_y;
float c_magnetom_z;
float MAG_Heading;
float Accel_Vector[3]= {
0,0,0}; //Store the acceleration in a vector
float Gyro_Vector[3]= {
0,0,0};//Store the gyros turn rate in a vector
float Omega_Vector[3]= {
0,0,0}; //Corrected Gyro_Vector data
float Omega_P[3]= {
0,0,0};//Omega Proportional correction
float Omega_I[3]= {
0,0,0};//Omega Integrator
float Omega[3]= {
0,0,0};
// Euler angles
float roll;
float pitch;
float yaw;
float errorRollPitch[3]= {
0,0,0};
float errorYaw[3]= {
0,0,0};
unsigned int counter=0;
byte gyro_sat=0;
float DCM_Matrix[3][3]= {
{
1,0,0 }
,{
0,1,0 }
,{
0,0,1 }
};
float Update_Matrix[3][3]={
{
0,1,2 }
,{
3,4,5 }
,{
6,7,8 }
}; //Gyros here
float Temporary_Matrix[3][3]={
{
0,0,0 }
,{
0,0,0 }
,{
0,0,0 }
};
//-+-+-+>>>> END POLOLU MINIMU - 9DOF Sensor section<<<<<<-+-+-+
//-+-+-+>>>> START Ultrasonic Sensor section<<<<<<-+-+-+
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); // NewPing setup of pins and maximum distance.
const int dangerThresh = 10; //threshold for obstacles (in cm)
int leftDistance, rightDistance, centerDistance; //distances on either side
//-+-+-+>>>> END Ultrasonic Sensor section<<<<<<-+-+-+
//-+-+-+>>>> Start Tac sensor contact section<<<<<<-+-+-+
int leftWiskerPin = 31;
int rightWiskerPin = 30;
int oldLeftWiskerValue;
int oldRightWiskerValue;
int numOfLeftWiskerhits = 0;
int numOfRightWiskerhits = 0;
int leftWiskerValue;
int rightWiskerValue;
//-+-+-+>>>> END Tac sensor contact section<<<<<<-+-+-+
//-+-+-+>>>> Start US PING averaging section<<<<<<-+-+-+
//------------------Avrageing-----------------------------------
const int numReadings = 5;
int readings[numReadings]; // the readings from the analog input
int index = 0; // the index of the current reading
int total = 0; // the running total
int average = 0; // the average
//------------------Avrageing-----------------------------------
//-+-+-+>>>> END US PING averaging section<<<<<<-+-+-+
//-+-+-+>>>> START GPS INIT section<<<<<<-+-+-+
TinyGPS Tgps;
float flat, flon;
//-+-+-+>>>> END GPS INIT section<<<<<<-+-+-+
//-+-+-+>>>> Start VT100 INIT section<<<<<<-+-+-+
//-+-+-+>>>> END VT100 INIT section<<<<<<-+-+-+
void setup()
{
Serial.begin(57600);
Serial2.begin(57600); //GPS INPUT (LocoSYS LS20031)
//**************Start Transiever (NRF24L01) config**************
Serial.println("Radio Booting ... "); // Print some stuff to serial
Mirf.cePin = 48;
Mirf.csnPin = 49;
Mirf.spi = &MirfHardwareSpi;
Mirf.init();
Mirf.setRADDR((byte *)"robot");
Mirf.payload = 16 * sizeof(byte);
Mirf.config();
Serial.println("Radio UP ... "); // Print some stuff to serial
//**************End Transiever (NRF24L01) config**************
//-+-+-+>>>> Start Tac sensor contact section<<<<<<-+-+-+
pinMode(leftWiskerPin, INPUT);
pinMode(rightWiskerPin, INPUT);
digitalWrite(leftWiskerPin, HIGH);
digitalWrite(rightWiskerPin, HIGH);
//-+-+-+>>>> END Tac sensor contact section<<<<<<-+-+-+
//-+-+-+>>>> START MotorDriver Vars section<<<<<<-+-+-+
pinMode(DIR_A, OUTPUT);
pinMode(PWM_A, OUTPUT);
pinMode(ENA_A, OUTPUT);
pinMode(DIR_B, OUTPUT);
pinMode(PWM_B, OUTPUT);
pinMode(ENA_B, OUTPUT);
//-+-+-+>>>> START MotorDriver Vars section<<<<<<-+-+-+
//-+-+-+>>>> Start US PING averaging section<<<<<<-+-+-+
for (int thisReading = 0; thisReading < numReadings; thisReading++)
readings[thisReading] = 0;
//-+-+-+>>>> END US PING averaging section<<<<<<-+-+-+
//-+-+-+>>>> Start VT100 INIT section<<<<<<-+-+-+
Serial.print(BYB);
Serial.println(CLR);
//-+-+-+>>>> END VT100 INIT section<<<<<<-+-+-+
//-+-+-+>>>> START POLOLU MINIMU - 9DOF Sensor section<<<<<<-+-+-+
Serial.println("IMU INIT ... "); // Print some stuff to serial
I2C_Init();
delay(1500);
Accel_Init();
Compass_Init();
Gyro_Init();
delay(20);
for(int i=0;i<32;i++) // We take some readings...
{
Read_Gyro();
Read_Accel();
for(int y=0; y<6; y++) // Cumulate values
AN_OFFSET[y] += AN[y];
delay(20);
}
for(int y=0; y<6; y++)
AN_OFFSET[y] = AN_OFFSET[y]/32;
AN_OFFSET[5]-=GRAVITY*SENSOR_SIGN[5];
for(int y=0; y<6; y++)
delay(2000);
timer=millis();
delay(20);
counter=0;
Serial.println("IMU Stream active ... "); // Print some stuff to serial
//-+-+-+>>>> END POLOLU MINIMU - 9DOF Sensor section<<<<<<-+-+-+
}