void
histogram_print(histogram_t *t)
{
uint16_t i, j;
for (i = 0; i < t->n_baselines; i++) {
rprintfFloat(4, ((double) t->min + (double) (i * t->baseline)) / t->precision);
rprintf(" to ");
rprintfFloat(4, ((double) t->min + (double) ((i+1) * t->baseline)) / t->precision);
rprintf(": ");
for (j = 0; j < t->histogram[i]; j++) {
rprintf("*");
}
rprintf("\n");
}
}
void _gpsDump(const GPS_COMMON* device){
const GPS* data = &device->info;
rprintf("Valid:%c\n", (data->valid) ? 'Y' : 'N');
rprintf("Satellites:%d\n",data->numSatellites);
rprintf("\nLongitude(degrees):"); rprintfFloat(10,data->longitude);
rprintf("\nLatitude(degrees):"); rprintfFloat(10,data->latitude);
rprintf("\nTrack(degrees):"); rprintfFloat(10,data->track);
rprintf("\nAltitude(m):"); rprintfFloat(10,data->altitude);
rprintf("\nSpeed(knots):"); rprintfFloat(10,data->speed);
rprintf("\nTime:"); rprintfFloat(10,data->fixTime);
rprintf("\n");
}
//----- Begin Code ------------------------------------------------------------
int main(void)
{
// init a few string variables:
char *welcome_msg1 = " LC Meter III ";
char *welcome_msg2 = "dansworkshop.com";
char *cal_message1 = "Switch to Cal/C ";
char *cal_message2 = "for calibration.";
char *cal_message3 = "calibrating... ";
char *blank_lcd_line = " ";
// initialize LCD
lcdInit();
// init timers and I/O:
init();
sei(); // enable global interrupts
// direct printf output to LCD
rprintfInit(lcdDataWrite);
// print vanity message on LCD for a second:
rprintfStr(welcome_msg1);
lcdGotoXY(0,1);
rprintfStr(welcome_msg2);
_delay_ms(1000);
// initialize the UART (serial port)
uartInit();
// make all rprintf statements use uart for output
rprintfInit(uartSendByte);
// print a little intro message so we know things are working
rprintfStr(welcome_msg1);
rprintf("\r\nhttp://www.");
rprintfStr(welcome_msg2);
rprintf("\r\n");
// instruct about setting mode switch to C for calibration
if(bit_is_clear(PIND, 4)) {
rprintfInit(lcdDataWrite);
lcdGotoXY(0,0);
rprintfStr(cal_message1);
lcdGotoXY(0,1);
rprintfStr(cal_message2);
rprintfInit(uartSendByte);
rprintfStr(cal_message1);
rprintfStr(cal_message2);
rprintf("\r\n");
}
while(bit_is_clear(PIND, 4)) {
// wait for user to switch mode to C
}
// send out the calibration message:
rprintfStr(cal_message3);
rprintf("\r\n");
rprintfInit(lcdDataWrite);
lcdGotoXY(0,0);
rprintfStr(cal_message3);
lcdGotoXY(0,1);
rprintfStr(blank_lcd_line);
rprintfInit(uartSendByte);
_delay_ms(1600);
F1 = running; // get open frequency
rprintfNum(10, 10, 0, ' ', F1 * 5);
rprintf("\r\n");
sbi(PORTD, PD3); // energize relay
_delay_ms(1000); // stabilize
F2 = running; // get test frequency
rprintfNum(10, 10, 0, ' ', F2 * 5);
rprintf("\r\n");
cbi(PORTD, PD3); // turn off relay
// do some floating point:
Cs = square(F2 * 5) * (.00000000092 / (square(F1 * 5) - square(F2 * 5))); // this should fit in a 64-bit value
Ls = 1 / (4 * square(M_PI) * square(F1 * 5) * Cs);
// everything out of the lcd for now:
rprintfInit(lcdDataWrite);
// enable PC5 as output
sbi(DDRC, PC5);
while (1) {
_delay_ms(200);
lcdGotoXY(0,0);
if(bit_is_clear(PIND, 4)) { // inductance measurement mode
if(running < 3) {
rprintf("Not an inductor \r");
} else {
Lt = (square(F1 * 5) / (square(running * 5)) - 1) * Ls;
rprintf("Lx: ");
if(Lt > .0001) {
rprintfFloat(4, Lt * 1000);
rprintf("mH");
}
else if(Lt > .0000001) {
rprintfFloat(4, Lt * 1000000);
rprintf("uH");
}
else {
rprintfFloat(4, Lt * 1000000000);
rprintf("nH");
}
rprintf(" \r");
}
} else { // capacitance measurement mode
if(running < 300) {
rprintf("Not a capacitor \r");
} else {
Ct = (square(F1 * 5) / (square(running * 5)) - 1) * Cs;
rprintf("Cx: ");
if(Ct > .0001) {
rprintfFloat(4, Ct * 1000);
rprintf("mF");
}
else if(Ct > .0000001) {
rprintfFloat(4, Ct * 1000000);
rprintf("uF");
}
else if(Ct > .0000000001){
rprintfFloat(4, Ct * 1000000000);
rprintf("nF");
}
else {
rprintfFloat(4, Ct * 1000000000000);
rprintf("pF");
}
rprintf(" \r");
}
}
if(bit_is_clear(PIND, 7)) {
lcdGotoXY(0,0);
rprintf("zeroed \r");
lcdGotoXY(0,1);
rprintfStr(blank_lcd_line);
lcdGotoXY(0,0);
F1 = running;
}
while(bit_is_clear(PIND, 7)) {
// do nothing till the user lets go of the zero button
}
// display the
lcdGotoXY(0,1);
rprintfNum(10, 6, 0, ' ', running * 5);
rprintf("Hz ");
}
return 0;
}
/*******************************************************************
* rprintf_test
*******************************************************************/
void rprintf_test(void)
{
u16 val;
u08 mydata;
u08 mystring[10];
double b;
u08 small;
u16 medium;
u32 big;
// initialize the UART (serial port)
uartInit();
// set the baud rate of the UART for our debug/reporting output
uartSetBaudRate(38400);
// initialize rprintf system
// - use uartSendByte as the output for all rprintf statements
// this will cause all rprintf library functions to direct their
// output to the uart
// - rprintf can be made to output to any device which takes characters.
// You must write a function which takes an unsigned char as an argument
// and then pass this to rprintfInit like this: rprintfInit(YOUR_FUNCTION);
rprintfInit(uartSendByte);
// initialize vt100 library
vt100Init();
// clear the terminal screen
vt100ClearScreen();
while (!(getkey() == 1)) //do the folling block until enter is pressed
{
// print a little intro message so we know things are working
rprintf("\r\nWelcome to rprintf Test!\r\n");
// print single characters
rprintfChar('H');
rprintfChar('e');
rprintfChar('l');
rprintfChar('l');
rprintfChar('o');
// print a constant string stored in FLASH
rprintfProgStrM(" World!");
// print a carriage return, line feed combination
rprintfCRLF();
// note that using rprintfCRLF() is more memory-efficient than
// using rprintf("\r\n"), especially if you do it repeatedly
mystring[0] = 'A';
mystring[1] = ' ';
mystring[2] = 'S';
mystring[3] = 't';
mystring[4] = 'r';
mystring[5] = 'i';
mystring[6] = 'n';
mystring[7] = 'g';
mystring[8] = '!';
mystring[9] = 0; // null termination
// print a null-terminated string from RAM
rprintfStr(mystring);
rprintfCRLF();
// print a section of a string from RAM
// - start at index 2
// - print 6 characters
rprintfStrLen(mystring, 2, 6);
rprintfCRLF();
val = 24060;
mydata = 'L';
// print a decimal number
rprintf("This is a decimal number: %d\r\n", val);
// print a hex number
rprintf("This is a hex number: %x\r\n", mydata);
// print a character
rprintf("This is a character: %c\r\n", mydata);
// print hex numbers
small = 0x12; // a char
medium = 0x1234; // a short
big = 0x12345678; // a long
rprintf("This is a 2-digit hex number (char) : ");
rprintfu08(small);
rprintfCRLF();
rprintf("This is a 4-digit hex number (short): ");
rprintfu16(medium);
rprintfCRLF();
rprintf("This is a 8-digit hex number (long) : ");
rprintfu32(big);
rprintfCRLF();
// print a formatted decimal number
// - use base 10
// - use 8 characters
// - the number is signed [TRUE]
// - pad with '.' periods
rprintf("This is a formatted decimal number: ");
rprintfNum(10, 8, TRUE, '.', val);
rprintfCRLF();
b = 1.23456;
// print a floating point number
// use 10-digit precision
// NOTE: TO USE rprintfFloat() YOU MUST ENABLE SUPPORT IN global.h
// use the following in your global.h: #define RPRINTF_FLOAT
rprintf("This is a floating point number: ");
rprintfFloat(8, b);
rprintfCRLF();
}
}
/*****************************************************************************
* Balance -
*****************************************************************************/
void balance(void)
{
unsigned long TimerMsWork;
long int g_bias = 0;
double x_offset = 532; //offset value 2.56V * 1024 / 4.93V = 4254
double q_m = 0.0;
double int_angle = 0.0;
double x = 0.0;
double tilt = 0.0;
int pwm;
InitADC();
init_pwm();
// initialize the UART (serial port)
uartInit();
// set the baud rate of the UART for our debug/reporting output
uartSetBaudRate(115200);
// initialize rprintf system
rprintfInit(uartSendByte);
// initialize vt100 library
vt100Init();
// clear the terminal screen
vt100ClearScreen();
TimerMsWork = TimerMsCur();
DDRB |= (1 << PB0); // Make B0 an output for LED
/* as a 1st step, a reference measurement of the angular rate sensor is
* done. This value is used as offset compensation */
for (int i=1 ; i<=200; i++) // determine initial value for bias of gyro
{
g_bias = g_bias + GetADC(gyro_sensor);
}
g_bias = g_bias / 200;
while (!(getkey() == 1))
{
/* insure loop runs at specified Hz */
while (!TimerCheck(TimerMsWork, (dt_PARAM * 1000) -1))
;
TimerMsWork = TimerMsCur();
// toggle pin B0 for oscilloscope timings.
PORTB = PINB ^ (1 << PB0);
// get rate gyro reading and convert to deg/sec
// q_m = (GetADC(gyro_sensor) - g_bias) / -3.072; // -3.07bits/deg/sec (neg. because forward is CCW)
q_m = (GetADC(gyro_sensor) - g_bias) * -0.3255; // each bit = 0.3255 /deg/sec (neg. because forward is CCW)
state_update(q_m);
// get Accelerometer reading and convert to units of gravity.
// x = (GetADC(accel_sensor) - x_offset) / 204.9; // (205 bits/G)
x = (GetADC(accel_sensor) - x_offset) * 0.00488; // each bit = 0.00488/G
// x is measured in multiples of earth gravitation g
// therefore x = sin (tilt) or tilt = arcsin(x)
// for small angles in rad (not deg): arcsin(x)=x
// Calculation of deg from rad: 1 deg = 180/pi = 57.29577951
tilt = 57.29577951 * (x);
kalman_update(tilt);
int_angle += angle * dt_PARAM;
rprintf(" x:");
rprintfFloat(8, x);
rprintf(" angle:");
rprintfFloat(8, angle);
rprintf(" rate:");
rprintfFloat(8, rate);
// Balance. The most important line in the entire program.
// balance_torque = Kp * (current_angle - neutral) + Kd * current_rate;
// rprintf("bal_torq: ");
// rprintfFloat(8, balance_torque);
// rprintfCRLF();
//steer_knob = 0;
// change from current angle to something proportional to speed
// should this be the abs val of the cur speed or just curr speed?
//double steer_cmd = (1.0 / (1.0 + Ksteer2 * fabs(current_angle))) * (Ksteer * steer_knob);
//double steer_cmd = 0.0;
// Get current rate of turn
//double current_turn = left_speed - right_speed; //<-- is this correct
//double turn_accel = current_turn - prev_turn;
//prev_turn = current_turn;
// Closed-loop turn rate PID
//double steer_cmd = KpTurn * (current_turn - steer_desired)
// + KdTurn * turn_accel;
// //+ KiTurn * turn_integrated;
// Possibly optional
//turn_integrated += current_turn - steer_cmd;
// Differential steering
//left_motor_torque = balance_torque + steer_cmd; //+ cur_speed + steer_cmd;
//right_motor_torque = balance_torque - steer_cmd; //+ cur_speed - steer_cmd;
// Limit extents of torque demand
//left_motor_torque = flim(left_motor_torque, -MAX_TORQUE, MAX_TORQUE);
// if (left_motor_torque < -MAX_TORQUE) left_motor_torque = -MAX_TORQUE;
// if (left_motor_torque > MAX_TORQUE) left_motor_torque = MAX_TORQUE;
//right_motor_torque = flim(right_motor_torque, -MAX_TORQUE, MAX_TORQUE);
// if (right_motor_torque < -MAX_TORQUE) right_motor_torque = -MAX_TORQUE;
// if (right_motor_torque > MAX_TORQUE) right_motor_torque = MAX_TORQUE;
pwm = (int) ((angle -3.5) * Kp) + (rate * Kd); // + (int_angle * Ki);
rprintf(" pwm:%d\r\n", pwm);
// Set PWM values for both motors
SetLeftMotorPWM(pwm);
SetRightMotorPWM(pwm);
}
SetLeftMotorPWM(0);
SetRightMotorPWM(0);
}
void gpsInfoPrint(void)
{
rprintfProgStrM("TOW: "); rprintfFloat(8, GpsInfo.TimeOfWeek.f); rprintfCRLF();
rprintfProgStrM("WkNum: "); rprintfNum(10,4,0,' ',GpsInfo.WeekNum); rprintfCRLF();
rprintfProgStrM("UTCoffset:"); rprintfFloat(8, GpsInfo.UtcOffset.f); rprintfCRLF();
rprintfProgStrM("Num SVs: "); rprintfNum(10,4,0,' ',GpsInfo.numSVs); rprintfCRLF();
rprintfProgStrM("X_ECEF: "); rprintfFloat(8, GpsInfo.PosECEF.x.f); rprintfCRLF();
rprintfProgStrM("Y_ECEF: "); rprintfFloat(8, GpsInfo.PosECEF.y.f); rprintfCRLF();
rprintfProgStrM("Z_ECEF: "); rprintfFloat(8, GpsInfo.PosECEF.z.f); rprintfCRLF();
rprintfProgStrM("TOF: "); rprintfFloat(8, GpsInfo.PosECEF.TimeOfFix.f); rprintfCRLF();
rprintfProgStrM("Updates: "); rprintfNum(10,6,0,' ',GpsInfo.PosECEF.updates); rprintfCRLF();
//u08 str[20];
//rprintfProgStrM(" PosLat: "); rprintfStr(dtostrf(GpsInfo.PosLat.f, 10, 5, str));
rprintfProgStrM("PosLat: "); rprintfFloat(8, 180*(GpsInfo.PosLLA.lat.f/PI)); rprintfCRLF();
rprintfProgStrM("PosLon: "); rprintfFloat(8, 180*(GpsInfo.PosLLA.lon.f/PI)); rprintfCRLF();
rprintfProgStrM("PosAlt: "); rprintfFloat(8, GpsInfo.PosLLA.alt.f); rprintfCRLF();
rprintfProgStrM("TOF: "); rprintfFloat(8, GpsInfo.PosLLA.TimeOfFix.f); rprintfCRLF();
rprintfProgStrM("Updates: "); rprintfNum(10,6,0,' ',GpsInfo.PosLLA.updates); rprintfCRLF();
rprintfProgStrM("Vel East: "); rprintfFloat(8, GpsInfo.VelENU.east.f); rprintfCRLF();
rprintfProgStrM("Vel North:"); rprintfFloat(8, GpsInfo.VelENU.north.f); rprintfCRLF();
rprintfProgStrM("Vel Up: "); rprintfFloat(8, GpsInfo.VelENU.up.f); rprintfCRLF();
// rprintfProgStrM("TOF: "); rprintfFloat(8, GpsInfo.VelENU.TimeOfFix.f); rprintfCRLF();
rprintfProgStrM("Updates: "); rprintfNum(10,6,0,' ',GpsInfo.VelENU.updates); rprintfCRLF();
rprintfProgStrM("Vel Head: "); rprintfFloat(8, GpsInfo.VelHS.heading.f); rprintfCRLF();
rprintfProgStrM("Vel Speed:"); rprintfFloat(8, GpsInfo.VelHS.speed.f); rprintfCRLF();
// rprintfProgStrM("TOF: "); rprintfFloat(8, GpsInfo.VelHS.TimeOfFix.f); rprintfCRLF();
rprintfProgStrM("Updates: "); rprintfNum(10,6,0,' ',GpsInfo.VelHS.updates); rprintfCRLF();
}
