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;
}
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
// Entry point
int main(int argc, char **argv)
{
/* Variable Declaration */
int i;
double v, voltage, timer, parts[2];
byte data[20], data0[2], data1[2], data2[2], data3[2], data4[2], data5[2], data6[2];
FILE *file1;
const char *filename1 = "testdata.txt";
unsigned char file_data[100];
time_t rawtime;
struct tm *timeinfo;
/* Execution */
printf("Inside main \n");
// the charger is connected to I2C bus-1 on RPi
init_i2c("/dev/i2c-1");
/* Set the registers of charger */
// first assignment tells the address - total 7 registers (0-6)
// the next assignment gives the actual value to be stored in decimal format
// To understand the value -- convert to binary - pad zeros on the left to make 8 bits -
// each bit represents value for corresponding regitser bit B0 - B7
data0[0] = 0;
data0[1] = 148; // 16; //144; // make the voltage permanent
data1[0] = 1;
data1[1] = 64;
data2[0] = 2;
data2[1] = 4;
data3[0] = 3;
data3[1] = 70;
data4[0] = 4;
data4[1] = 42;
data5[0] = 5;
data5[1] = 0;
data6[0] = 6;
data6[1] = 112;
// Open the file in read-only mode
file1 = fopen(filename1, "rb");
if(file1 != NULL)
{
/* Some more variables */
char line[1000];
char *part;
int q = 0, delV = 0, vBatCode, vBatRegValue, totalDelay = 0;
/* read a line from file */
while(fgets(line, sizeof line, file1) != NULL)
{
printf("%s", line); // print the line contents to stdout
// parse the line to get time and voltage
part = strtok(line, ",");
while(part != NULL)
{
// convert the extracted part to float
parts[q] = atof(part);
// continue parsing
part = strtok(NULL, ",");
q++;
}
// reset the array counter for next line
q = 0;
printf("Time: %lf, voltage: %lf \n", parts[0], parts[1]);
/* Logic for converting voltage to code as needed by the charger */
// Get voltage over 3.5V, in milivolts
delV = parts[1] * 1000 - 3500;
// Regularize bad voltages, no voltage allowed under 3.5 V and over 4.44 V
if (delV < 0)
{
delV = 0;
}
else if (delV > 900)
{
delV = 900;
}
// Our resolution is 20 mV
vBatCode = delV / 20;
// as the first two bits are zero, pg - 33 of datasheet
vBatRegValue = vBatCode * 4;
// The zero problem, the voltage starts with 3.6, even trying to set it to 3.5, so rather, starting with 3.52
if(vBatRegValue == 0)
{
data2[1] = 4; //vBatRegValue;
}
else
{
data2[1] = vBatRegValue;
}
// This is the time, from the text file.
totalDelay = parts[0];
setRegisters: // GOTO should be used **very rarely**
// Find the time of register write
time (&rawtime);
timeinfo = localtime(&rawtime);
printf("Current local time and data: %s \n", asctime(timeinfo));
printf("Setting voltage to: %lf \n", 3500.0 + delV);
// Write data to the I2C device, one register at a time
printf("data0[1] : %d, data1[1]: %d, data2[1]: %d, data3[1]: %d \n", data0[1], data1[1], data2[1], data3[1]);
printf("data4[1] : %d, data5[1]: %d, data6[1]: %d \n", data4[1], data5[1], data6[1]);
I2cSendData(BQ24261_ADDR, data0, 2);
I2cSendData(BQ24261_ADDR, data1, 2);
I2cSendData(BQ24261_ADDR, data2, 2);
I2cSendData(BQ24261_ADDR, data3, 2);
I2cSendData(BQ24261_ADDR, data4, 2);
I2cSendData(BQ24261_ADDR, data5, 2);
I2cSendData(BQ24261_ADDR, data6, 2);
/* Timing Logic */
// The voltage value needs to be re-written to the charger every few seconds (10 here)
while (totalDelay > 0)
{
// using delay, makes this platform specific
if (totalDelay > 10)
{
totalDelay = totalDelay - 10;
delay(10000);
goto setRegisters;
}
else if (totalDelay <= 10)
{
delay(totalDelay * 1000);
totalDelay = 0;
}
}
}
// Close the file
fclose(file1);
printf("\n\n\n *****Charging done !!!*****");
}
else
{
perror(filename1);
}
printf("\n\n\n *****Charging done !!!*****");
close(deviceDescriptor);
endwin();
return 0;
}
