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; } } }
void gyroScreen(void){ 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,WHITE); f3d_lcd_drawString(chW,chH,"Y: ",BLACK,WHITE); f3d_lcd_drawString(chW,2*chH,"Z: ",BLACK,WHITE); //Create bar graph labels f3d_lcd_drawString(width/2-18, 45, "X-Axis", BLACK, WHITE); f3d_lcd_drawString(width/2-18, 65, "Y-Axis", BLACK, WHITE); f3d_lcd_drawString(width/2-18, 85, "Z-Axis", BLACK, WHITE); //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); */ f3d_gyro_getdata(axis); printf("X: %f\n", axis[0]); sprintf(data, "%f", axis[0]); f3d_lcd_drawString(4*chW,0,data,BLACK,WHITE); sprintf(data, "%f", axis[1]); f3d_lcd_drawString(4*chW,chH,data,BLACK,WHITE); sprintf(data, "%f", axis[2]); f3d_lcd_drawString(4*chW,2*chH,data,BLACK,WHITE); //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); }
int main(void) { f3d_lcd_init(); f3d_gyro_init(); f3d_uart_init(); setvbuf(stdin, NULL, _IONBF, 0); setvbuf(stdout, NULL, _IONBF, 0); setvbuf(stderr, NULL, _IONBF, 0); float ray[3]; int prev[3]; prev[0]=0; prev[1]=0; prev[2]=0; char xaxis[] = "X-axis"; char yaxis[] = "Y-axis"; char zaxis[] = "Z-axis"; char *xptr,*yptr,*zptr; xptr=xaxis; yptr=yaxis; zptr=zptr; /// layout & labels \\\ f3d_lcd_fillScreen(WHITE); //X-axis f3d_lcd_fillSection(64,22,64,44,BLACK); f3d_lcd_drawString(50,55,xaxis,BLACK,WHITE); //Y-axis f3d_lcd_fillSection(64,66,64,88,BLACK); f3d_lcd_drawString(50,99,yaxis,BLACK,WHITE); //Z-axis f3d_lcd_fillSection(64,110,64,132,BLACK); f3d_lcd_drawString(50,143,zaxis,BLACK,WHITE); while(1){ f3d_gyro_getdata(ray); /// xaaaaans \\\ //X-axis double xval = (double) ray[0]; if(xval <=0){ //negative bar double xp = xval/-400; int xbp = (int) 63*xp; //number of pixels on x axis to show int delta = 63-xbp; //x1 starting pixel if(xbp > prev[0]){ /*check if its greater/smaller than prevRay*/ f3d_lcd_fillSection(delta,22,63,44,BLUE); /*increase bar size; fill with Blue*/ }else{ f3d_lcd_fillSection(0,22,delta,44,WHITE); /*decrease bar size; fill with White*/ } prev[0] = xbp; }else{ //positive bar double xp = xval/400; int xbp = (int) 65*xp; int delta = 65+xbp; if(xbp > prev[0]){ /*check if greater/smaller than prevRay*/ f3d_lcd_fillSection(65,22,delta,44,BLUE); /*increase bar size; fill with Blue*/ }else{ f3d_lcd_fillSection(delta,22,128,44,WHITE); /*decrease bar size; fill white*/ } prev[0] = xbp; } //Y-axis double yval = (double) ray[1]; if(yval <=0){ //negative bar double yp = yval/-400; int ybp = (int) 63*yp; int delta = 63-ybp; if(ybp > prev[0]){ f3d_lcd_fillSection(delta,66,63,88,GREEN); }else{ f3d_lcd_fillSection(0,66,delta,88,WHITE); } prev[1] = ybp; }else{ //positive bar double yp = yval/400; int ybp = (int) 65*yp; int delta = 65+ybp; if(ybp > prev[1]){ f3d_lcd_fillSection(65,66,delta,88,GREEN); }else{ f3d_lcd_fillSection(delta,66,128,88,WHITE); } prev[1] = ybp; } //Z-axis double zval = (double) ray[2]; if(zval <=0){ //negative bar double zp = zval/-400; int zbp = (int) 63*zp; int delta = 63-zbp; if(zbp > prev[0]){ f3d_lcd_fillSection(delta,110,63,132,YELLOW); }else{ f3d_lcd_fillSection(0,110,delta,132,WHITE); } prev[2] = zbp; }else{ //positive bar double zp = zval/400; int zbp = (int) 65*zp; int delta = 65+zbp; if(zbp > prev[1]){ f3d_lcd_fillSection(65,110,delta,132,YELLOW); }else{ f3d_lcd_fillSection(delta,110,128,132,WHITE); } prev[2] = zbp; } } }