int main(int argc, char **argv)
{
int i, v;
byte data[20];
init_i2c("/dev/i2c-1");
// set mode register to 1 to activate accelerometer
data[0] = 7;
data[1] = 1;
I2cSendData(MMA7660_ADDR,data,2);
printf("Hit any key to quit\n\n");
while (1) {
I2cReadData(MMA7660_ADDR,data,11);
for (i=0; i<3; i++) {
v = (data[i]/2^6)*9.81;
if (v>=0x20) v = -(0x40 - v); //sign extend negatives
printf("%c:%3d ",i+'X',v);
}
for (i=3; i<11; i++)
printf("%x: %02x ",i,data[i]);
printf("\r");
}
printf("\n");
close(deviceDescriptor);
return 0;
}
int main(int argc, char **argv)
{
int i;
byte data[2];
init_i2c("/dev/i2c-0");
I2cReadData(0x38,data,1);
printf("data1 %x\n",data[0]);
printf("data2 %x\n",data[1]);
printf("data3 %x\n",data[2]);
return 0;
}
/**
* This function reads data from the PHY.
*
* @param I2cLibPtr contains a pointer to the instance of the IIC library
* @param PhyAddr is the address of PHY to be read from
* @param Reg is the register address to be read from
* @param Data is the pointer which stores the data read
* @param SlaveAddr is the address of the slave we are sending to.
*
* @return XST_SUCCESS if successful else XST_FAILURE.
*
******************************************************************************/
int I2cPhyRead(XIIC_LIB *I2cLibPtr, u8 PhyAddr, u8 Reg, u16 *Data, u16 SlaveAddr)
{
int Status;
u8 WrBuffer[2];
u8 RdBuffer[2];
WrBuffer[0] = Reg;
Status = I2cWriteData(I2cLibPtr, WrBuffer, 1, SlaveAddr);
if (Status != XST_SUCCESS) {
xil_printf("PhyWrite: Writing data failed\n\r");
return Status;
}
Status = I2cReadData(I2cLibPtr, RdBuffer, 2, SlaveAddr);
if (Status != XST_SUCCESS) {
xil_printf("PhyRead: Reading data failed\n\r");
return Status;
}
*Data = RdBuffer[0] << 8 | RdBuffer[1];
return Status;
}
void DaThread::run()
{
///////////////////////////////////////////////
//general variables
int counter =0;
unsigned int micro_seconds=22000;
int gain=5;
int count=0;
double inVal=0;
///////////////////////////////////////////////
//this is for the accelerometer use
int i, v;
byte data[20];
init_i2c("/dev/i2c-1");
// set mode register to 1 to activate accelerometer
data[0] = 7;
data[1] = 1;
I2cSendData(MMA7660_ADDR,data,2);
///////////////////////////////////////////////
//this is for the servos
// I primarily got the structure of the code from TutorialsPoint website C++ Multithreading, Qthreads I
// think I read somewhere are object oriented "helpers" of pthreads
uint64_t rs_time=1900;
int ls_boolean = 1;
uint64_t ls_time=1100;
int rs_boolean = 1;
bcm2835_init();
pthread_t servo_threads[2];
struct servo_thread_data threads_data[2];
//pin BCM18 should be the first thread delay of below
//delay should be less than 1500 mS
//pin 18 is the left side looking from the back of the vehicle
threads_data[0].pin_num=PIN2;
threads_data[0].hightime=&ls_time;//ls_time<1500
threads_data[0].keepgoing=&ls_boolean;
threads_data[0].id=0;
//pin BCM17
threads_data[1].pin_num=PIN;
threads_data[1].hightime=&rs_time;//ls_time<1500
threads_data[1].keepgoing=&rs_boolean;
threads_data[1].id=1;
//servo running with pin BCM18
pthread_create(&servo_threads[0],NULL,rotateServo, (void *)&threads_data[0]);
//servo running with pin BCM
pthread_create(&servo_threads[1],NULL,rotateServo, (void *)&threads_data[1]);
//////////////////////////////////////////////////
//while loop reads, calculates on the reading, and should adjust PWM for servos
double v_cm_per_sec=0;
double intermediate_double=0;
while(counter<100000){
//read in data and then in for loop , FORMAT THE DATA!
I2cReadData(MMA7660_ADDR,data,11);
for (i=0; i<3; i++) {
//v = (data[i]/2^6)*9.81;
v=data[i];
v=v&0x0000003F;//int datatype has 4 bytes , set the most significant 26 bits to 0.
if (v>=0x20)
{
v = -(0x40 - v);
} //sign extend negatives <- this clever way was found online
else{
v=v&0x0000001F;
// v= (v<<3)/8; //<- this other clever way was found in mbed's library
}
intermediate_double=v/21.33; // range for signed 5 bits is -32 to 31. The accel.
// from +-1.5g. So, +32/1.5=21.33 approx.
//I really looked at mbed's library for the MMA7660, but I think
// the math works out
//look at consel for all three values, X,Y,Z.
// we only focus on one access, the forward direction assuming, the accelerometer is parallel to ground
if(i==0){intermediate_double-=.046882;} //subtracting "x bias" value to get artificial value
if(i==1){intermediate_double-=.093675;} //subtracting "y bias" value to get artificial value
//held the accelerometer stationary to get these approx
//values
printf("%c in g's:%f ",i+'X',intermediate_double);
if(i==0){inVal=intermediate_double;}
}
printf("\n");
//inVal is in g's right now
//Physics:vf= velocity_initial + deltaV;
// where deltaV= a*delta_time or
// deltaV= inVal*9.81*(micro_seconds/1000000.0)*100
// inVal*9.81 converst to m/s^2 then multiply by delta_time or (micro_seconds/1000000.0) to get m/s
// and then multiply this by 100 to get cm/s
v_cm_per_sec=v_cm_per_sec + inVal*9.81*(micro_seconds/1000000.0)*100;
inVal=v_cm_per_sec;
//adjust speed of servo motors
if (inVal<TARGET_VELOCITY){ //speed up
ls_time-=100;
rs_time+=100;
}
else if(inVal>TARGET_VELOCITY){//slow down
ls_time-=100;
rs_time+=100;
}
counter = counter+1;
//SLEEP because we don't want a very large amount of points
//taken from StackOverflow
//http://stackoverflow.com/questions/4184468/sleep-for-milliseconds
// std::this_thread::sleep_for(std::chrono::milliseconds(1000))
usleep(micro_seconds);
//inVal = gain * sin( M_PI * count/50.0 );
++count;
// add the new input to the plot-Taken from Professor
memmove( yDataPointer, yDataPointer+1, (DATA-1) * sizeof(double) );
yDataPointer[DATA-1] = inVal;
} //end of while
//the while loops in the servo thread should exit shortly after setting the booleans below to 0
rs_boolean=0;
ls_boolean=0;
}//end of run function
int main(void)
{
/* Configure the oscillator both devices */
ConfigureOscillator();
/* Initialize IO ports and peripherals for both devices */
InitGPIO();
InitUART();
InitI2c();
InitI2cCompass();
/* Program for the bracelet */
#ifdef PROTECTED
/* Initialize IO ports and peripherals */
InitTimerUS();
/* The values of the magnetic field will be save in x and y */
s16 x = 0;
s16 y = 0;
while(1)
{
I2cReadData(&x, &y);
ComputeAngle(&angle, x, y);
PutData16(angle);
__delay_ms(500);
}
#endif
/* Program for the bodyguard*/
#ifdef BODY_GUARD
/* Initialize IO ports and peripherals */
InitADC();
InitPWM();
InitLcd();
InitTimerServo();
/* TODO <INSERT USER APPLICATION CODE HERE> */
u16 ADC_values[NMB_SENSORS];
u16 average[NMB_SENSORS];
u8 i;
u8 j;
/* The values of the magnetic field will be save in x and y */
s16 x = 0;
s16 y = 0;
u16 angle2=0;
char T[5];
memset(ADC_values,0x00,sizeof(ADC_values));
memset(average, 0x00,sizeof(average));
/*
for(i=0; i<NMB_SENSORS; i++)
{
ADC_values[i]=0;
}*/
LcdClear();
#if MAGNETIC_SENSOR
while(1)
{
I2cReadData(&x, &y);
angle2=((-atan2(x,y)*180)/3.14)+180;
/* Computes the angle using the arctan2 which provides an angle
* between -180° and 180°, then converts the result that is in radian
* into degree (*180/pi) and in the end add 180° so the angle is between
* 0° and 360° */
//LcdPutFloat(angle2,0);
LcdPutFloat(angle2,0);
LcdGoto(1,2);
LcdPutFloat(angle,0);
__delay_ms(500);
LcdClear();
}
#endif
#ifdef BODY_GUARD_MODE
while(1)
{
for(j=0; j<NMB_MEASURES; j++)
{
/* SENSORS SAMPLING */
for(i=0; i<NMB_SENSORS; i++)
{
StartADC(ADC_values);
}
ObjectDetection(ADC_values, average);
}
LcdClear();
LcdPutFloat(CCP2RB, 0);
//__delay_ms(1000);
LcdClear();
/* Set Flags */
ObjectReaction(average);
DistanceFlag(average[US]);
/* react */
AutoBodyGuard();
}
#endif
#ifdef AUTO_FLEE
while(1)
{
for(j=0; j<NMB_MEASURES; j++)
{
/* SENSORS SAMPLING */
for(i=0; i<NMB_SENSORS; i++)
{
StartADC(ADC_values);
}
ObjectDetection(ADC_values, average);
}
LcdClear();
/* Set Flags */
ObjectReaction(average);
//DistanceFlag(average[US]);
AutoFLee();
}
#endif
#endif
return 0;
}
