void drawIndicator(int x, int y, int onoff){
int i,j;
for(i=0;i<5;i++){
for(j=0;j<5;j++){
if(onoff){
f3d_lcd_drawPixel(x+i,y+j,GREEN);
}
else
f3d_lcd_drawPixel(x+i,y+j,RED);
}
}
}
void drawStraightupLine(int color) {
// draw the compass straight line
int row;
for (row = 0; row < centerY; row++) {
f3d_lcd_drawPixel(centerX, row, color);
}
}
void f3d_lcd_drawChar(uint8_t x, uint8_t y, unsigned char c, uint16_t color, uint16_t background_color) {
int i, j;
for (i = 0; i < 5; i++) {
for (j = 0; j < 8; j++){
f3d_lcd_drawPixel(x+i,y+j, background_color);
}
}
for (i = 0; i < 5; i++) {
uint8_t byte = ASCII[c*5 + i];
for (j = 0; j < 8; j++){
if (byte & (1)) {
f3d_lcd_drawPixel(x+i,y+j, color);
}
byte >>= 1;
}
}
}
// Prints the given asteroid to the screen (IK... they're cirlces...)
void print_asteroid(asteroid_t t, uint16_t color) {
int x;
int y;
for (x = -(t.radius); x < t.radius; x++) {
int height = (int)sqrt(t.radius * t.radius - x * x);
for (y = -height; y < height; y++)
f3d_lcd_drawPixel(x + t.x, y + t.y, color);
}
}
// draw a fixed-sized rectangle starting at origin x, y for the gyroscope app
void drawGyroRect(int x, int y, int color) {
int rowBound = y + RECT_LENGTH;
int colBound = x + RECT_WIDTH;
int r, c;
for (r = y; r < rowBound; r++) {
for (c = x; c < colBound; c++) {
f3d_lcd_drawPixel(r, c, color);
}
}
}
// draw a fixed-sized rectangle starting at origin x, y
void drawRect(int x1, int y1, int x2, int y2, int color) {
int colBound = y2;
int rowBound = x2;
int r, c;
for (r = x1; r < rowBound; r++) {
for (c = y1; c < colBound; c++) {
f3d_lcd_drawPixel(r, c, color);
}
}
}
int main(void) {
// Set buffer to protect initializations
setvbuf(stdin, NULL, _IONBF, 0);
setvbuf(stdout, NULL, _IONBF, 0);
setvbuf(stderr, NULL, _IONBF, 0);
//Initialize peripherals
f3d_gyro_init();
f3d_uart_init();
f3d_lcd_init();
int chW = 6;
int chH = 10;
int width = ST7735_width;
char data[8];
int i, j;
int dataX;
int dataY;
int dataZ;
//Array to store gyro data
float axis[3];
//Fill background to RED
f3d_lcd_fillScreen2(BLUE);
//Set up data labels
f3d_lcd_drawString(chW,0,"X: ",BLACK,BLUE);
f3d_lcd_drawString(chW,chH,"Y: ",BLACK,BLUE);
f3d_lcd_drawString(chW,2*chH,"Z: ",BLACK,BLUE);
//Create bar graph labels
f3d_lcd_drawString(width/2-18, 45, "X-Axis", WHITE, BLUE);
f3d_lcd_drawString(width/2-18, 65, "Y-Axis", WHITE, BLUE);
f3d_lcd_drawString(width/2-18, 85, "Z-Axis", WHITE, BLUE);
//Labels to show which color is which value
f3d_lcd_drawString(chW, 110, "Positive", RED, BLUE);
f3d_lcd_drawString(width-9*chW, 110, "Negative", GREEN, BLUE);
//Create the bar graph box
int top;
int side;
f3d_lcd_drawString(3, 130, "Loading...",WHITE, BLUE);
for (top = 3; top < ST7735_width-3; top++) {
for (side = 40; side < 103; side++) {
f3d_lcd_drawPixel(top, 40, WHITE);
f3d_lcd_drawPixel(top, 41, WHITE);
f3d_lcd_drawPixel(top, 101, WHITE);
f3d_lcd_drawPixel(top, 102, WHITE);
f3d_lcd_drawPixel(2, side, WHITE);
f3d_lcd_drawPixel(3, side, WHITE);
f3d_lcd_drawPixel(ST7735_width-2, side, WHITE);
f3d_lcd_drawPixel(ST7735_width-3, side, WHITE);
}
}
f3d_lcd_drawString(64, 130, "Done!",WHITE, BLUE);
//Loop, collecting gyro data
while(1)
{
f3d_gyro_getdata(axis);
printf("X: %f\n", axis[0]);
sprintf(data, "%f", axis[0]);
f3d_lcd_drawString(4*chW,0,data,BLACK,BLUE);
sprintf(data, "%f", axis[1]);
f3d_lcd_drawString(4*chW,chH,data,BLACK,BLUE);
sprintf(data, "%f", axis[2]);
f3d_lcd_drawString(4*chW,2*chH,data,BLACK,BLUE);
//360/120 = 3 so we divided magnitude by 3
//X-axis point graph
dataX = (int)axis[0] / 3.0;
blockGen(dataX, 55);
// Y-axis point graph
dataY = (int)axis[1] / 3.0;
blockGen(dataY, 75);
// Z-axis point graph
dataZ = (int)axis[2] / 3.0;
blockGen(dataZ, 95);
}
}
//making block of pixels
void blockGen(int mag, int y){
int start;
if (mag >= 0){
for (start = 5; start <= 123; start++){
if (start <= mag){
//draw block
f3d_lcd_drawPixel(start, y, RED);
f3d_lcd_drawPixel(start, y+1, RED);
f3d_lcd_drawPixel(start, y+2, RED);
f3d_lcd_drawPixel(start, y+3, RED);
f3d_lcd_drawPixel(start, y+4, RED);
} else {
//remove block by BG color
f3d_lcd_drawPixel(start, y, BLUE);
f3d_lcd_drawPixel(start, y+1, BLUE);
f3d_lcd_drawPixel(start, y+2, BLUE);
f3d_lcd_drawPixel(start, y+3, BLUE);
f3d_lcd_drawPixel(start, y+4, BLUE);
}
}
}
//For negative magnitudes
if (mag <= 0){
mag = mag * -1;
for (start = 5; start <= 123; start++){
if (start <= mag){
f3d_lcd_drawPixel(start, y, GREEN);
f3d_lcd_drawPixel(start, y+1, GREEN);
f3d_lcd_drawPixel(start, y+2, GREEN);
f3d_lcd_drawPixel(start, y+3, GREEN);
f3d_lcd_drawPixel(start, y+4, GREEN);
} else {
f3d_lcd_drawPixel(start, y, BLUE);
f3d_lcd_drawPixel(start, y+1, BLUE);
f3d_lcd_drawPixel(start, y+2, BLUE);
f3d_lcd_drawPixel(start, y+3, BLUE);
f3d_lcd_drawPixel(start, y+4, BLUE);
}
}
}
}
int main(void) {
//various initializations
f3d_uart_init();
f3d_i2c1_init();
delay(10);
f3d_accel_init();
delay(10);
f3d_mag_init();
delay(10);
f3d_gyro_init();
delay(10);
f3d_nunchuk_init();
delay(10);
f3d_user_btn_init();
f3d_lcd_init();
//reset pixels by filling screen RED
f3d_lcd_fillScreen(RED);
//set buffers
setvbuf(stdin, NULL, _IONBF, 0);
setvbuf(stdout, NULL, _IONBF, 0);
setvbuf(stderr, NULL, _IONBF, 0);
// initialize gyro constants
START_X = (ST7735_width / 2) - (RECT_WIDTH / 2);
START_Y = (ST7735_height / 2) - (RECT_LENGTH / 2);
GYRO_UPPER_BOUND = 120;
X_MARGIN = (ST7735_width - RECT_WIDTH) / 2;
Y_MARGIN = (ST7735_height - RECT_LENGTH) / 2;
//set centerX and centerY by using global varaibles from library
centerX = ST7735_width / 2;
centerY = ST7735_height / 2;
// constants for the PITCHROLL_MODE
const int barGraphWidth = 40;
const int rollStartY = 30;
const int pitchStartY = 120;
//variables for keeping track of data from previous point
int prevRollX = 0, prevRollY = 0;
int prevPitchX = 0, prevPitchY = 0;
int prevGyroRow = START_X, prevGyroCol = START_Y;
//set float arrays for accel and mag data
float accel_buffer[3];
float mag_buffer[3];
float gyro_buffer[3];
nunchuk_t nunchuk_data;
nunchuk_t *nunchuk_ptr = &nunchuk_data;
//start board in compass mode
int app_mode = COMPASS_MODE;
char *app_mode_title;
while(1) {
//retrieve accel and mag data and insert into their buffers
f3d_accel_read(accel_buffer);
f3d_mag_read(mag_buffer);
f3d_gyro_getdata(gyro_buffer);
f3d_nunchuk_read(nunchuk_ptr);
delay(10);
float Ax1 = accel_buffer[0];
float Ay1 = accel_buffer[1];
float Az1 = accel_buffer[2];
//calcuation of sum of squares
float A_sum_squares = sqrt(Ax1 * Ax1 + Ay1 * Ay1 + Az1 * Az1);
//calculate pitch using accel data and atan2
float pitch = atan2(Ax1, sqrt(Ay1 * Ay1 + Az1 * Az1));
//calculate roll in a similar manner
float roll = atan2(Ay1, sqrt(Ax1 * Ax1 + Az1 * Az1));
//feed mag buffers mag x, y, and z
float Mx = mag_buffer[0];
float My = mag_buffer[1];
float Mz = mag_buffer[2];
//calculate heading in degrees
float Xh = Mx * cos(pitch) + Mz * sin(pitch);
float Yh = Mx * sin(roll) * sin(pitch) + My * cos(roll) - Mz * sin(roll) * cos(pitch);
float headingDegrees = rad_to_deg(atan2(Yh, Xh));
// convert heading degrees to a circular system
float newHeadingDegrees = 0.0;
if (headingDegrees > 90.0) {
// quad I
newHeadingDegrees = fabsf(headingDegrees - 180.0);
} else if (headingDegrees > 0.0) {
// quad II
newHeadingDegrees = 180.0 - headingDegrees;
} else {
// quads III and IV
newHeadingDegrees = 180.0 + fabsf(headingDegrees);
}
int prev_app_mode = app_mode;
// change mode based on nunchuk
const unsigned char joystick_epsilon = 50;
int c_pressed = nunchuk_ptr->z;
int z_pressed = nunchuk_ptr->c;
if (c_pressed != z_pressed) {
// decide based on buttons
if (c_pressed) {
// go right
app_mode = (app_mode + 1) % 4;
} else {
// go left
app_mode = (app_mode + 3) % 4;
}
} else {
// decide based on joystick
const int joystick_x_center = 141;
int joystick_delta = nunchuk_ptr->jx - joystick_x_center;
if (abs(joystick_delta) >= joystick_epsilon) {
// only switch app mode if joystick change is significant
if (joystick_delta < 0) {
// go right
app_mode = (app_mode + 1) % 4;
} else {
// go left
app_mode = (app_mode + 3) % 4;
}
}
}
if (app_mode != prev_app_mode) {
f3d_lcd_fillScreen(RED);
}
//define variables for row and columns of type int
int row, col;
switch(app_mode) {
case COMPASS_MODE: // compass mode
f3d_lcd_fillScreen(RED);
f3d_lcd_drawString(0, 0, "Compass", WHITE, RED);
//draw static white line point upwards on LCD
drawStraightupLine(WHITE);
const float radius = 30.0;
float theta = deg_to_rad(newHeadingDegrees) - (M_PI / 2.0);
//calculat x and y offset
float xOffset = radius * cos(theta);
float yOffset = radius * sin(theta);
//set second point
int x2 = centerX + ((int) xOffset);
int y2 = centerY + ((int) yOffset);
//draw point on the screen at location x2 and y2
f3d_lcd_drawPixel(x2, y2, CYAN);
break;
case PITCHROLL_MODE: // tilt mode
app_mode_title = "Board";
pitchroll_label:
// erase old bars
drawRect(0, rollStartY, prevRollX, prevRollY, RED);
drawRect(0, pitchStartY, prevPitchX, prevPitchY, RED);
//draw the word "Roll" on upper left of LCD
f3d_lcd_drawString(0, 0, "Roll", CYAN, RED);
// title the application
f3d_lcd_drawString((int) ST7735_width * 0.65, 0, app_mode_title, CYAN, RED);
//set color for redrawing of the bars
int rollColor = (roll < 0.0) ? MAGENTA : CYAN;
//calculate perceentage using fabsf (absolute value for float)
float rollPercentage = fabsf(roll) / M_PI;
//calculate rollX and rollY for drawing the roll bar
int rollX = rollPercentage * ST7735_width;
int rollY = rollStartY + barGraphWidth;
drawRect(0, rollStartY, rollX, rollY, rollColor);
//draw the word pitch 90 pixels below Roll
f3d_lcd_drawString(0, 90, "Pitch", CYAN, RED);
//set color for redrawing
int pitchColor = (pitch < 0.0) ? MAGENTA : CYAN;
//calculate pitchPercentage using fabsf(absolute for float)
float pitchPercentage = fabsf(pitch) / M_PI;
//calculate pitchX and pitch Y for drawing Pitch rectangle
int pitchX = pitchPercentage * ST7735_width;
int pitchY = pitchStartY + barGraphWidth;
drawRect(0, pitchStartY, pitchX, pitchY, pitchColor);
//keep track of RollX and PitchX for loop
prevRollX = rollX; prevRollY = rollY;
prevPitchX = pitchX; prevPitchY = pitchY;
break;
case GYRO_MODE: // gyro mode
f3d_lcd_fillScreen(RED);
float gyroDataAvg = (gyro_buffer[0] + gyro_buffer[1] + gyro_buffer[2]) / 3.0f;
float percentageOffset = gyroDataAvg / GYRO_UPPER_BOUND;
int xPixelsFromCenter = (percentageOffset * X_MARGIN) / 1;
int yPixelsFromCenter = (percentageOffset * Y_MARGIN) / 1;
row = START_X + xPixelsFromCenter;
col = START_Y + yPixelsFromCenter;
prevGyroRow = row;
prevGyroCol = col;
drawGyroRect(col, row, WHITE);
break;
case NUNCHUK_MODE:
app_mode_title = "Nunchuk";
const int nunchuk_tilt_upperbound = 1023;
const int nunchuk_tilt_midpoint = nunchuk_tilt_upperbound / 2;
int ax = nunchuk_ptr->ax - nunchuk_tilt_midpoint;
int ay = nunchuk_ptr->ay - nunchuk_tilt_midpoint;
int az = nunchuk_ptr->az;
// calculate pitch and roll of nunchuk
pitch = atan2(ay, sqrt(pow(ay, 2) + pow(az, 2)));
roll = atan2(ax, sqrt(pow(ax, 2) + pow(az, 2)));
// double pitch and roll values to exaggerate their size on bar graph
pitch *= 2;
roll *= 2;
// all the rest is the same as board accelerometer application, so...
goto pitchroll_label;
break;
default:
break;
}
}
}
