int main(int argc, char *argv[])
{
char filename[20];
int magRaw[3];
//Open the i2c bus
sprintf(filename, "/dev/i2c-%d", 1);
file = open(filename, O_RDWR);
if (file<0) {
printf("Unable to open I2C bus!");
exit(1);
}
//Select the magnetomoter
if (ioctl(file, I2C_SLAVE, MAG_ADDRESS) < 0) {
printf("Error: Could not select magnetometer\n");
}
//Enable the magnetometer
writeMagReg( CTRL_REG5_XM, 0b11110000); // Temp enable, M data rate = 50Hz
writeMagReg( CTRL_REG6_XM, 0b01100000); // +/-12gauss
writeMagReg( CTRL_REG7_XM, 0b00000000); // Continuous-conversion mode
while(1)
{
readMAG(magRaw);
printf("magRaw X %i \tmagRaw Y %i \tMagRaw Z %i \n", magRaw[0],magRaw[1],magRaw[2]);
//Only needed if the heading value does not increase when the magnetometer is rotated clockwise
magRaw[1] = -magRaw[1];
//Compute heading
float heading = 180 * atan2(magRaw[1],magRaw[0])/M_PI;
//Convert heading to 0 - 360
if(heading < 0)
heading += 360;
printf("heading %7.3f \t ", heading);
//Sleep for 0.25ms
usleep(25000);
}
}
int main(int argc, char *argv[])
{
char filename[20];
int magRaw[3];
signal(SIGINT, INThandler);
//Open the i2c bus
sprintf(filename, "/dev/i2c-%d", 1);
file = open(filename, O_RDWR);
if (file<0) {
printf("Unable to open I2C bus!");
exit(1);
}
//Select the magnetomoter
if (ioctl(file, I2C_SLAVE, MAG_ADDRESS) < 0) {
printf("Error: Could not select magnetometer\n");
}
//Enable the magnetometer
writeMagReg( CTRL_REG5_XM, 0b11110000); // Temp enable, M data rate = 50Hz
writeMagReg( CTRL_REG6_XM, 0b01100000); // +/-12gauss
writeMagReg( CTRL_REG7_XM, 0b00000000); // Continuous-conversion mode
while(1)
{
readMAG(magRaw);
printf("magXmax %4i magYmax %4i magZmax %4i magXmin %4i magYmin %4i magZmin %4i\n", magXmax,magYmax,magZmax,magXmin,magYmin,magZmin);
if (magRaw[0] > magXmax) magXmax = magRaw[0];
if (magRaw[1] > magYmax) magYmax = magRaw[1];
if (magRaw[2] > magZmax) magZmax = magRaw[2];
if (magRaw[0] < magXmin) magXmin = magRaw[0];
if (magRaw[1] < magYmin) magYmin = magRaw[1];
if (magRaw[2] < magZmin) magZmin = magRaw[2];
//Sleep for 0.25ms
usleep(25000);
}
}
int main(int argc, char *argv[])
{
char filename[20];
int magRaw[3];
int accRaw[3];
float accXnorm,accYnorm,pitch,roll,magXcomp,magYcomp;
//Open the i2c bus
sprintf(filename, "/dev/i2c-%d", 1);
file = open(filename, O_RDWR);
if (file<0) {
printf("Unable to open I2C bus!");
exit(1);
}
//Select the magnetomoter
if (ioctl(file, I2C_SLAVE, MAG_ADDRESS) < 0) {
printf("Error: Could not select magnetometer\n");
}
// Enable accelerometer.
writeAccReg(CTRL_REG1_XM, 0b01100111); // z,y,x axis enabled, continuos update, 100Hz data rate
writeAccReg(CTRL_REG2_XM, 0b00100000); // +/- 16G full scale
//Enable the magnetometer
writeMagReg( CTRL_REG5_XM, 0b11110000); // Temp enable, M data rate = 50Hz
writeMagReg( CTRL_REG6_XM, 0b01100000); // +/-12gauss
writeMagReg( CTRL_REG7_XM, 0b00000000); // Continuous-conversion mode
while(1)
{
readMAG(magRaw);
readACC(accRaw);
//If your IMU is upside down, comment out the two lines below which we correct the tilt calculation
//accRaw[0] = -accRaw[0];
//accRaw[1] = -accRaw[1];
//Compute heading
float heading = 180 * atan2(magRaw[1],magRaw[0])/M_PI;
//Convert heading to 0 - 360
if(heading < 0)
heading += 360;
printf("heading %7.3f \t ", heading);
//Normalize accelerometer raw values.
accXnorm = accRaw[0]/sqrt(accRaw[0] * accRaw[0] + accRaw[1] * accRaw[1] + accRaw[2] * accRaw[2]);
accYnorm = accRaw[1]/sqrt(accRaw[0] * accRaw[0] + accRaw[1] * accRaw[1] + accRaw[2] * accRaw[2]);
//Calculate pitch and roll
pitch = asin(accXnorm);
roll = -asin(accYnorm/cos(pitch));
//Calculate the new tilt compensated values
magXcomp = magRaw[0]*cos(pitch)+magRaw[02]*sin(pitch);
magYcomp = magRaw[0]*sin(roll)*sin(pitch)+magRaw[1]*cos(roll)-magRaw[2]*sin(roll)*cos(pitch);
//Calculate heading
heading = 180*atan2(magYcomp,magXcomp)/M_PI;
//Convert heading to 0 - 360
if(heading < 0)
heading += 360;
printf("Compensated Heading %7.3f \n", heading);
//Sleep for 25ms
usleep(25000);
}
}
void* getData(void* myParameter)
{
int *Pacc_raw;
int *Pmag_raw;
int acc_raw[3];
int mag_raw[3];
float magXmax,magXmin;
float magYmax,magYmin;
float magZmax,magZmin;
float compass;
int i = 0;
float AccXangle = 0.0;
float AccYangle = 0.0;
float MagX = 0.0;
float MagY = 0.0;
float MagZ = 0.0;
float Xbefore = 250.0;
float Ybefore = 250.0;
float Zbefore = 250.0;
float heading;
Pacc_raw=acc_raw;
Pmag_raw=mag_raw;
int gyroYbuff,gyroXbuff;
char filename[20];
sprintf(filename, "/dev/i2c-%d", 1);
file = open(filename, O_RDWR);
if (file<0) {
printf("Unable to open I2C bus! \n");
exit(1);
}
printf("I2C Openned \n");
enableACC();
enableMAG();
while(EnableSensor)
{
readACC(Pacc_raw);
readMAG(Pmag_raw);
AccXangle = (float) (atan2(*(acc_raw+1),*(acc_raw+2))+M_PI)*RAD_TO_DEG;
AccYangle = (float) (atan2(*(acc_raw+2),*acc_raw)+M_PI)*RAD_TO_DEG;
//MagX= (float) *(mag_raw)+482;
//MagZ= (float) *(mag_raw+1)+783;
//MagY= (float) *(mag_raw+2);
//if (MagX<magXmin){magXmin=MagX;}else if(MagX>magXmax){magXmax=MagX;}
//if (MagY<magYmin){magYmin=MagY;}else if(MagY>magYmax){magYmax=MagY;}
//if (MagZ<magZmin){magZmin=MagZ;}else if(MagZ>magZmax){magZmax=MagZ;}
//printf (" MagX \e[m %7.3f \t -- MagY %7.3f \t -- MagZ %7.3f \t \n",MagX,MagY,MagZ);
//printf (" MinX \e[m %7.3f \t -- MinY %7.3f \t -- MinZ %7.3f \t \n",magXmin,magYmin,magZmin);
//printf (" MaxX \e[m %7.3f \t -- MaxY %7.3f \t -- MaxZ %7.3f \t \n",magXmax,magYmax,magZmax);
if ((AccYangle < 185) && (AccYangle > 100)) {condition=2;}
else if ((AccYangle > 335)&&(AccYangle < 361)||((AccYangle >0)&& (AccYangle < 50))) {condition=1;}
else {condition=0;}
//printf (" AccXangle \e[m %7.3f \t -- AccYangle %7.3f \t -- AccZangle %7.3f \t \n",AccXangle,AccYangle,AccZangle);
if (i==10){virtualGyroX=0;virtualGyroY=0;i=0;}
virtualGyroX=virtualGyroX+(Xbefore-AccXangle);
virtualGyroY=virtualGyroY+(Ybefore-AccYangle);
Xbefore=AccXangle; Ybefore=AccYangle;
//conAction();
usleep(10000);
i++;
}
return 0;
}
int main(int argc, char** argv)
{ //
int select = 102;
int Tdelay = 10 ;
long Mdelay = 10000 ;
// Switch statement control variable
int cnt = 30000 ;
int dutcycl = 24000 ;
int mot = 5000 ;
int period = 3125 ;
int pos = 0 ;
int position = 16000 ;
int stat = 0 ; // return status
int valu = 0 ; //returned value
int catch = 500 ;
char IMU_buf[96]={0x55} ;
int IMU_val[6] ; // IMU 3 axis value
long int nanoseconds=0 ;
long int dutcyc = 0 ;
int PWM_Positions[10] = {16000,18000,20000,22000,24000,26000,28000,30000,32000,16000} ;
// Initialization begins here
system.init() ;
dateTime.init() ;
_TRISE6 = 0; // configure port as output
_RE6 = 1; // set the output (active high) Turn on 6Volt supply
// Init I21C2
// Init Servo motor
PWM9_init() ;
SET_PWM9_Period(period) ;
SET_PWM9_DutyCycle(dutcycl) ;
// Init A2D Channel 10- motor current
// A2D_c10_init() ;
Analog_init() ;
// i2c2_TalkToDevice(RTC_ADDR, RTC_SET_LENGTH, STG_SET_TIME, 0, 0) ;
i2c2_init(75) ; // I2C2_KHz
i2c2_reset_bus() ;
while (cnt>0)
{ switch (select)
{ case 0 :
while (mot>0)
{ pos=0 ;
while (pos <9)
{ position=PWM_Positions[pos];
SET_PWM9_DutyCycle(position) ;
delay(Mdelay) ;
pos++ ;
}
--mot ;
}
break ;
case 1 :
valu = Read_Switch_Align() ;
break ;
case 2 :
valu = Read_Switch_Home() ;
break ;
case 3 :
nanoseconds= RSP78_init () ;
break ;
case 4 :
dutcyc = RSP78_Read_Sig_Period() ;
break ;
case 5 :
valu = A2D_c10_read() ;
break ;
case 6 :
valu = A2D_c10_sample(5,1) ;
break ;
case 7 :
SET_PWM9_Period( period) ;
break ;
case 8 :
SET_PWM9_DutyCycle(dutcycl) ;
break ;
case 9 :
break ;
// *********************************************
// Camera 1 Testing
// ******************************************
case 10 :
initializeCam1() ;
break ;
case 11 :
setOutputCam1(0) ;
break ;
case 12 :
setOutputCam1(1) ;
break ;
case 13 :
setOffCam1() ;
break ;
case 14 :
setOnCam1() ;
break ;
// *********************************************
// Camera 2 Testing
// *****************************************
case 20 :
initializeCam2() ;
break ;
case 21 :
setOutputCam2(0) ;
break ;
case 22 :
setOutputCam2(1) ;
break ;
case 23 :
setOffCam2() ;
break ;
case 24 :
setOnCam2() ;
break ;
// *********************************************
// LED 1 Testing
// *********************************************
case 30 :
initializeLED1() ;
break ;
case 31 :
setOutputLED1(0) ;
break ;
case 32 :
setOutputLED1(1) ;
break ;
case 33 :
setOffLED1() ;
break ;
case 34 :
setOnLED1() ;
break ;
// *********************************************
// LED2 Testing
// *********************************************
case 40 :
initializeLED2() ;
break ;
case 41 :
setOutputLED2(0) ;
break ;
case 42 :
setOutputLED2(1) ;
break ;
case 43 :
setOffLED2() ;
break ;
case 44 :
setOnLED2() ;
break ;
// *********************************************
// Servo 1 Testing
// *********************************************
case 50 :
initializeServo() ;
break ;
case 51 :
setOutputServo(0) ;
break ;
case 52 :
setOutputServo(1) ;
break ;
case 53 :
setOffServo() ;
break ;
case 54 :
setOnServo() ;
break ;
// *********************************************
// PING I2C2 Device Testing
// *********************************************
case 60 :
i2c2_TalkToDevice(I2C_CLR_DIAG, 0, NullPtr, 0, NullPtr) ; // Clear Diagn ostic Countrers
break ;
case 61 :
stat=i2c2_TalkToDevice(RTC_ADDR, 0, NullPtr, 0, NullPtr) ;
break ;
case 62 :
setI2C2_RTCTime(RTC_I2C_TimeStampData) ; // RGB Gesture
select = 63 ;
break ;
case 63 :
getI2C2_RTCTime(RTC_I2C_TimeStampData) ; //Temp Humidity Pressure
break ;
case 64 :
stat=i2c2_TalkToDevice(0x6B, 0, NullPtr, 0, NullPtr) ;
break ;
case 65 :
stat=i2c2_TalkToDevice(RTC_ADDR, 0, NullPtr, 0, NullPtr) ;
stat=i2c2_TalkToDevice(0x39, 0, NullPtr, 0, NullPtr) ;
stat=i2c2_TalkToDevice(0x77, 0, NullPtr, 0, NullPtr) ;
stat=i2c2_TalkToDevice(0x1D, 0, NullPtr, 0, NullPtr) ;
stat=i2c2_TalkToDevice(0x6B, 0, NullPtr, 0, NullPtr) ;
break ;
case 66 :
// dt=datetime.get ;
break ;
case 69 :
stat=i2c2_TalkToDevice(RTC_ADDR, 0, NullPtr, 0, NullPtr) ; // Real Time Clock Set Harmon Time
stat=i2c2_TalkToDevice(0x39, 0, NullPtr, 0, NullPtr) ; // RGBC
stat=i2c2_TalkToDevice(0x77, 0, NullPtr, 0, NullPtr) ; // THP
stat=i2c2_TalkToDevice(0x1D, 0, NullPtr, 0, NullPtr) ; // Accel/Magne
stat=i2c2_TalkToDevice(0x6B, 0, NullPtr, 0, NullPtr) ; // Gyro
break ;
// *********************************************
// RTC I2C2 Test both Real Time Clocks
// *********************************************
case 70 :
// Clear Diagnostic Countrers
stat=i2c2_TalkToDevice(I2C_CLR_DIAG, 0, NullPtr, 0, NullPtr) ;
delay(1) ;
break ;
case 71 :
// Ping using talk to device
stat=i2c2_TalkToDevice(RTC_ADDR, 0, NullPtr, 0, NullPtr) ;
delay(1) ;
break ;
case 72 :
stat=i2c2_TalkToDevice(RTC_ADDR, RTC_GET_LENGTH, RTC_SET_TIME, RTC_RSP_LENGTH, RTC_GET_TIME) ; // get I2C RTC BCD string
delay(1) ;
break ;
case 73 :
stat=i2c2_TalkToDevice(RTC_ADDR, RTC_SET_LENGTH, RTC_SET_TIME, 0, NullPtr) ; // set I2C RTC BCD string
delay(1) ;
break ;
case 74 :
stat=getI2C2_RTCTime(RTC_I2C_TimeStampData) ; // Get I2C RTC into TimeStamp STring
delay(1) ;
break ;
case 75 :
stat = setI2C2_RTCTime(RTC_I2C_TimeStampData) ; // Set I2c RTC from TimeStamp STring
delay(1) ;
break ;
case 76 :
getTimeStamp(RTC_I2C_TimeStampData) ; // get PIC RTC into timestamp string
delay(1) ;
break ;
case 77 :
setPIC_RTCTime(RTC_I2C_TimeStampData) ; // set PIC RTC from timestamp string
delay(1) ;
break ;
case 78 :
delay(1) ;
break ;
case 79 :
stat=getI2C2_RTCTime(RTC_I2C_TimeStampData) ;
setPIC_RTCTime(RTC_I2C_TimeStampData) ;
delay(1) ;
break ;
// *********************************************
// GYR I2C2 Decive Testing
// *********************************************
case 80 :
// Clear the Diagnostic buffer
stat=i2c2_TalkToDevice(I2C_CLR_DIAG, 0, NullPtr, 0, NullPtr) ; // Clear Diagn ostic Countrers
delay (1) ;
break ;
case 81 :
stat=enableIMU() ;
delay (1) ;
break ;
case 82 :
stat= dumpIMU(IMU_buf) ;
delay (1) ;
break ;
case 83 :
stat= readACC(IMU_val) ;
delay (1) ;
break ;
case 84 :
stat=readMAG(IMU_val) ;
delay (1) ;
break ;
case 85 :
stat=readGYR(IMU_val) ;
delay (1) ;
break ;
case 86 :
stat=i2c2_TalkToDevice(ACC_Addr, 0, NullPtr, 0, NullPtr) ;
delay (1) ;
break ;
case 87 :
stat=gyro3_main(argc, argv) ;
delay (1) ;
break ;
case 88 :
stat=AMG5_main(argc, argv) ;
delay (1) ;
break ;
case 89 :
delay (1) ;
break ;
// *********************************************
// RGB I2C2 Decive Testing
// *********************************************
case 90 :
stat=i2c2_TalkToDevice(I2C_CLR_DIAG, 0, NullPtr, 0, NullPtr) ; // Clear Diagn ostic Countrers
RGB_Configure1 () ;
select=94 ; // read back values
break ;
case 91 :
RGB_Configure2 () ;
break ;
case 92 :
RGB_Configure3 () ;
break ;
case 93 :
RGB_ReadStatus () ;
break ;
case 94 :
RGB_ReadValues () ;
break ;
case 95 :
RGB_ClearIrqs () ;
break ;
case 99 :
stat=RGB_Configure1 () ;
stat=RGB_ReadStatus () ;
stat=RGB_ReadValues () ;
stat=RGB_ReadStatus () ;
stat=RGB_ClearIrqs () ;
break ;
// *********************************************
// THP I2C2 Device Testing
// *********************************************
case 100 :
stat= i2c2_TalkToDevice(THP_Addr,0, NullPtr, 0, NullPtr) ; // ping device
break ;
case 101 :
// i2c2_TalkToDevice(I2C_CLR_DIAG, 0, NullPtr, 0, NullPtr) ;
stat = THP_Configure1 () ;
// select=105 ; // read back values
break ;
case 102 :
i2c2_TalkToDevice(I2C_CLR_DIAG, 0, NullPtr, 0, NullPtr) ;
stat = THP_Configure2 () ;
select=106 ; // read back values
break ;
case 103 :
stat = THP_Configure3 () ;
break ;
case 104 :
// stat = THP_ReadStatus () ; // part of read values
// select=104 ; // read back values
break ;
case 105 :
// stat = THP_Diag_RdIrqs () ;
THP_Init () ;
break ;
case 106 :
stat = THP_Read_Values () ;
break ;
case 107 :
// stat = THP_Diag_RdIrqs () ;
break ;
case 108 :
// stat = THP_Init () ;
// stat = THP_Diag_RdIrqs () ;
break ;
case 109 :
stat = THP_Configure1 () ;
// THP_ReadStatus () ;
stat = THP_Read_Values () ;
// stat = THP_ReadStatus () ;
// THP_Diag_RdIrqs () ;
break ;
default:
delay(10) ;
} // end of switch
// *********************************************
// SWITCH ENDS
// *********************************************
if(cnt % catch==0)
{ delay(10) ;
}
delay (Tdelay) ;
cnt-- ;
} // end of while
return (0);
}
