gotta check ctrl algo - it gives negative value
Fork of BAE_hw_test1_2 by
Diff: ACS.cpp
- Revision:
- 3:20647ff68b3c
- Parent:
- 2:edd107ea4740
- Child:
- 5:4199c0dfed33
diff -r edd107ea4740 -r 20647ff68b3c ACS.cpp --- a/ACS.cpp Fri Dec 26 06:12:09 2014 +0000 +++ b/ACS.cpp Sat Jan 31 14:11:53 2015 +0000 @@ -25,7 +25,7 @@ { - printf("\nEnterd PWMGEN function\n"); + printf("\n\rEnterd PWMGEN function\n\r\r"); float DCx = 0; //Duty cycle of Moment in x, y, z directions float ix = 0; //Current sent in x, y, z TR's float timep = 0.02 ; @@ -57,7 +57,7 @@ } else if(ix >= 0.0624&& ix < 0.555) { - printf("\nACS entered if\n"); + printf("\n\rACS entered if\n\r"); DCx = 331.15*pow(ix,4) - 368.09*pow(ix,3) + 140.43*pow(ix,2) + 158.59*ix + 0.0338; PWM1.period(timep); PWM1 = DCx/100 ; @@ -73,7 +73,7 @@ // printf("!!!!!!!!!!Error!!!!!!!!!"); } - printf("\n moment :%f\n",DCx); + printf("\n\r moment :%f\n\r",DCx); float DCy = 0; //Duty cycle of Moment in x, y, z directions float iy = 0; //Current sent in x, y, z TR's @@ -104,7 +104,7 @@ } else if(iy >= 0.0624&& iy < 0.555) { - printf("\nACS entered if\n"); + printf("\n\rACS entered if\n\r"); DCy = 331.15*pow(iy,4) - 368.09*pow(iy,3) + 140.43*pow(iy,2) + 158.59*iy + 0.0338; PWM2.period(timep); PWM2 = DCy/100 ; @@ -149,7 +149,7 @@ } else if(iz >= 0.0624&& iz < 0.555) { - printf("\nACS entered if\n"); + printf("\n\rACS entered if\n\r"); DCz = 331.15*pow(iz,4) - 368.09*pow(iz,3) + 140.43*pow(iz,2) + 158.59*iz + 0.0338; PWM3.period(timep); PWM3 = DCz/100 ; @@ -165,7 +165,7 @@ // printf("!!!!!!!!!!Error!!!!!!!!!"); } -printf("\nExited PWMGEN function\n"); +printf("\n\rExited PWMGEN function\n\r"); } /*------------------------------------------------------------------------------------------------------------------------------------------------------- -------------------------------------------MAGNETOMETER-------------------------------------------------------------------------------------------------*/ @@ -181,7 +181,7 @@ DRDY = 0; int a ; a=DRDY; - printf("\n DRDY is %d\n",a); + printf("\n\r DRDY is %d\n\r",a); SSN_MAG=1; //pin is disabled spi_acs.format(8,0); // 8bits,Mode 0 spi_acs.frequency(100000); //clock frequency @@ -205,12 +205,12 @@ float* FUNC_ACS_MAG_EXEC() { - //printf("\nEntered magnetometer function\n"); + //printf("\n\rEntered magnetometer function\n\r"); //DRDY.output(); DRDY.write(0); int a; a = DRDY; - printf("\n DRDY is %d\n",a); + printf("\n\r DRDY is %d\n\r",a); SSN_MAG=0; //enabling slave to measure the values wait_ms(10); spi_acs.write(0x82); //initiates measurement @@ -229,7 +229,7 @@ wait_ms(5); if(DRDY==1) { - printf("\nwth\n"); + printf("\n\rwth\n"); SSN_MAG=0; spi_acs.write(0xc9); //command byte for retrieving data @@ -256,7 +256,7 @@ Bnewvalue[axis]=(float)Bvalue[axis]*22.0*pow(10.0,-3.0); //1 LSB=(22nT)...final value of field obtained in micro tesla wait_ms(10); - printf("\t%lf\n",Bnewvalue[axis]); + printf("\t%lf\n\r",Bnewvalue[axis]); } SSN_MAG=1; @@ -285,7 +285,7 @@ float Mu[2], z[2], dv[2], v[2], u[2], tauc[3] = {0, 0, 0}; //outputs float invJm[3][3]; float kmu2 = 0.07, gamma2 = 1.9e4, kz2 = 0.4e-2, kmu = 0.003, gamma = 5.6e4, kz = 0.1e-4; - printf("Entered cntrl algo\n"); + printf("Entered cntrl algo\n\r"); //structure parameters void inverse (float mat[3][3], float inv[3][3]); @@ -320,7 +320,7 @@ for(i=0;i<3;i++) { - printf("\nreached here\n"); + printf("\n\rreached here\n\r"); if(den!=0) //b[i]=b[i]/den; //there is a problem here. The code gets stuck here. Maf value is required ; @@ -384,7 +384,7 @@ } /////////// Output to Matlab ////////////////// /* for(i=0;i<3;i++) { - printf("%f\n",tauc[i]*10000000); + printf("%f\n\r",tauc[i]*10000000); wait_ms(10); } } @@ -423,7 +423,7 @@ } sscanf (tempchar, "%8x", &n); memcpy(&flval, &n, sizeof(long)); - printf("%f\n", flval); + printf("%f\n\r", flval); x[j] = flval; } }*/ @@ -532,7 +532,7 @@ float * FUNC_ACS_EXEC_GYR() { - printf("\nEntered gyro\n"); + printf("\n\rEntered gyro\n\r"); uint8_t response; uint8_t MSB,LSB; int16_t bit_data; @@ -574,7 +574,7 @@ { printf("%f\t",data[i]); //printing the angular velocity values } - printf("\n"); + printf("\n\r"); break; } drFlag=0; @@ -590,7 +590,7 @@ response=spi_acs.write(response|BIT_SLEEP); //setting the gyroscope in sleep mode ssn_gyr=1; wait(0.1); - printf("\nExited gyro\n"); + printf("\n\rExited gyro\n\r"); return data; }