Permalink
Browse files

still debugging

  • Loading branch information...
1 parent 36654b1 commit d8e03a825e1534bddc4c0c493302a0c1fb852ba3 @lin187 lin187 committed Mar 8, 2012
View
@@ -0,0 +1,116 @@
+void read_adc_raw(void)
+{
+ SensorData[0]= analogRead(3); // Gx
+ SensorData[1]= analogRead(4); // Gy Maps sensors to correct order
+ SensorData[2]= analogRead(5); // Gz No longer Very weak low pass filter
+ SensorData[3]= analogRead(0); // Ax there used to be .9 weight on new data
+ SensorData[4]= analogRead(1); // Ay and .1 weight on the previous data
+ SensorData[5]= analogRead(2); // Az
+
+}
+
+void read_adc_offset(void)
+{
+ int i = 0;
+ for(i = 0; i < 6; i++){
+ ProcessedSensorData[i] = (SensorData[i] - SensorData_Offset[i]) * SENSOR_SIGN[i];
+ }
+}
+
+int read_adc(int select)
+{
+ return (SensorData[select]-SensorData_Offset[select])*SENSOR_SIGN[select];
+}
+
+void printdata(void)//ToDeg(x)
+{
+ Serial.print("!!!");
+ #if PRINT_ANALOGS==1
+ Serial.print("AN0:");
+ Serial.print(read_adc(0));
+ Serial.print(",AN1:");
+ Serial.print(read_adc(1));
+ Serial.print(",AN2:");
+ Serial.print(read_adc(2));
+ Serial.print(",AN3:");
+ Serial.print(read_adc(3));
+ Serial.print (",AN4:");
+ Serial.print(read_adc(4));
+ Serial.print (",AN5:");
+ Serial.print(read_adc(5));
+ Serial.print (",");
+ #endif
+ #if PRINT_DCM == 1
+ Serial.print ("EX0:");
+ Serial.print(convert_to_dec(DCM_Matrix[0][0]));
+ Serial.print (",EX1:");
+ Serial.print(convert_to_dec(DCM_Matrix[0][1]));
+ Serial.print (",EX2:");
+ Serial.print(convert_to_dec(DCM_Matrix[0][2]));
+ Serial.print (",EX3:");
+ Serial.print(convert_to_dec(DCM_Matrix[1][0]));
+ Serial.print (",EX4:");
+ Serial.print(convert_to_dec(DCM_Matrix[1][1]));
+ Serial.print (",EX5:");
+ Serial.print(convert_to_dec(DCM_Matrix[1][2]));
+ Serial.print (",EX6:");
+ Serial.print(convert_to_dec(DCM_Matrix[2][0]));
+ Serial.print (",EX7:");
+ Serial.print(convert_to_dec(DCM_Matrix[2][1]));
+ Serial.print (",EX8:");
+ Serial.print(convert_to_dec(DCM_Matrix[2][2]));
+ Serial.print (",");
+ #endif
+ #if PRINT_EULER == 1
+ Serial.print("RLL:");
+ Serial.print(ToDeg(atan2(DCM_Matrix[2][1],DCM_Matrix[2][2])));
+ Serial.print(",PCH:");
+ Serial.print(ToDeg(asin(DCM_Matrix[2][0])));
+ Serial.print(",YAW:");
+ Serial.print(ToDeg(atan2(DCM_Matrix[1][0],DCM_Matrix[0][0])));
+ Serial.print (",");
+ #endif
+ #if PRINT_GPS == 1
+ Serial.print("LAT:");
+ Serial.print((long)(lat*10000000));
+ Serial.print(",LON:");
+ Serial.print((long)(lon*10000000));
+ Serial.print(",ALT:");
+ Serial.print(alt_MSL);
+ Serial.print(",COG:");
+ Serial.print((ground_course));
+ Serial.print(",SOG:");
+ Serial.print(ground_speed);
+ Serial.print(",FIX:");
+ Serial.print((int)gpsFix);
+ #endif
+
+ Serial.println("***");
+}
+
+long convert_to_dec(float x)
+{
+ return x*1000000;
+}
+/////
+
+
+//
+//void Analog_Reference(uint8_t mode)
+//{
+// analog_reference = mode;
+//}
+//ADC interrupt vector, this piece of
+//is executed everytime a convertion is done.
+//ISR(ADC_vect)
+//{
+// volatile uint8_t low, high;
+// low = ADCL;
+// high = ADCH;
+// analog_buffer[MuxSel]=(high << 8) | low;
+// MuxSel++;
+// if(MuxSel >=8) MuxSel=0;
+// ADMUX = (analog_reference << 6) | (MuxSel & 0x07);
+ // start the conversion
+// ADCSRA|= (1<<ADSC);
+//}
View
@@ -0,0 +1,40 @@
+//Computes the dot product of two vectors
+float Vector_Dot_Product(float vector1[3],float vector2[3])
+{
+ float op=0;
+
+ for(int c=0; c<3; c++)
+ {
+ op+=vector1[c]*vector2[c];
+ }
+
+ return op;
+}
+
+//Computes the cross product of two vectors
+void Vector_Cross_Product(float vectorOut[3], float v1[3],float v2[3])
+{
+ vectorOut[0]= (v1[1]*v2[2]) - (v1[2]*v2[1]);
+ vectorOut[1]= (v1[2]*v2[0]) - (v1[0]*v2[2]);
+ vectorOut[2]= (v1[0]*v2[1]) - (v1[1]*v2[0]);
+}
+
+//Multiply the vector by a scalar.
+void Vector_Scale(float vectorOut[3],float vectorIn[3], float scale2)
+{
+ for(int c=0; c<3; c++)
+ {
+ vectorOut[c]=vectorIn[c]*scale2;
+ }
+}
+
+void Vector_Add(float vectorOut[3],float vectorIn1[3], float vectorIn2[3])
+{
+ for(int c=0; c<3; c++)
+ {
+ vectorOut[c]=vectorIn1[c]+vectorIn2[c];
+ }
+}
+
+
+
View
@@ -0,0 +1,77 @@
+void StateSpaceControl()
+{
+ //multiply the k vector with the x vector to find control effort
+ if(j == 0){
+ if(k == 0){
+ math.MatrixMult((float*)x_1_2, (float*)kvector, 1, n, 1, (float*)&u);
+ }
+ else{
+ math.MatrixMult((float*)x_1_1, (float*)kvector, 1, n, 1, (float*)&u);
+ }
+ }
+ else{
+ if(k == 0){
+ math.MatrixMult((float*)x_2_2, (float*)kvector, 1, n, 1, (float*)&u);
+ }
+ else{
+ math.MatrixMult((float*)x_2_1, (float*)kvector, 1, n, 1, (float*)&u);
+ }
+ }
+
+ // determine motor direction
+ if (u >= 0) {
+ dir = 1;
+ }else{
+ dir = -1;
+ }
+ // Saturate output if greater than the max allowed
+ u = abs(u);
+ if (u > max_output) {
+ u = max_output;
+ }
+
+}
+
+// Actuate the motors based on u
+// u is the result of StateSpaceControl()
+void Actuate(void) {
+ // Motor1
+ if (j == 0) {
+ if (dir == -1) {
+ // actuate clockwise
+ u = map(u, 0, max_output, MOTOR_CW_LOW, MOTOR_CW_HIGH);
+
+ Motor1.write(u);
+ }
+ else if (dir == 1) {
+ // actuate counter clockwise
+ u = map(u, 0, max_output, MOTOR_CCW_HIGH, MOTOR_CCW_LOW);
+
+ Motor1.write(u);
+ }
+ if((millis() - motortime[j])>250){
+ Serial.println(max_output);
+ motortime[j]=millis();
+ }
+ }
+ // Motor 2
+ else if (j == 1) {
+ if (dir == -1) {
+ // actuate clockwise
+ u = map(u, 0, max_output, MOTOR_CW_LOW, MOTOR_CW_HIGH);
+
+ Motor2.write(u);
+ }
+ else if (dir == 1) {
+ // actuate counter clockwise
+ u = map(u, 0, max_output, MOTOR_CCW_HIGH, MOTOR_CCW_LOW);
+
+ Motor2.write(u);
+ }
+ if(millis()-motortime[j]>250){
+ Serial.println(max_output);
+ motortime[j]=millis();
+ }
+ }
+}
+
@@ -164,18 +164,23 @@ float H_t_2[n][m]={
};
-//float temp1[n][1];
-float temp2[n][n];
-//float temp3[n][n];
-float temp4[m][1];
-float tempmn[m][n] = {{0,0,0,0},{0,0,0,0},{0,0,0,0},{0,0,0,0},{0,0,0,0},{0,0,0,0}};
-float tempnm[n][m] = {{0,0,0,0,0,0},{0,0,0,0,0,0},{0,0,0,0,0,0},{0,0,0,0,0,0}};
-float temp5[n][m] = {{0,0,0,0,0,0},{0,0,0,0,0,0},{0,0,0,0,0,0},{0,0,0,0,0,0}};
-float tempm[m][m];
-//float tempm2[m][m];
+//uses the global variable j to switch between using 1 and 2
+void KalmanFilter()
+ {
+float temp1 [n][1]={{0},{0},{0},{0}};
+float temp2 [n][n]={{0,0,0,0},{0,0,0,0},{0,0,0,0},{0,0,0,0}};
+float temp3[n][n]={{0,0,0,0},{0,0,0,0},{0,0,0,0},{0,0,0,0}};
+
+float temp4[m][1]={{0},{0},{0},{0},{0},{0}};
+
+float tempmn[m][n]={{0,0,0,0},{0,0,0,0},{0,0,0,0},{0,0,0,0},{0,0,0,0},{0,0,0,0}};
+float tempnm[n][m]={{0,0,0,0,0,0},{0,0,0,0,0,0},{0,0,0,0,0,0},{0,0,0,0,0,0}};
+float tempnm2 [n][m]={{0,0,0,0,0,0},{0,0,0,0,0,0},{0,0,0,0,0,0},{0,0,0,0,0,0}};
+float tempm [m][m]={{0,0,0,0,0,0},{0,0,0,0,0,0},{0,0,0,0,0,0},{0,0,0,0,0,0},{0,0,0,0,0,0},{0,0,0,0,0,0}};
+float tempm2 [m][m]={{0,0,0,0,0,0},{0,0,0,0,0,0},{0,0,0,0,0,0},{0,0,0,0,0,0},{0,0,0,0,0,0},{0,0,0,0,0,0}};
float* X_k_1;
float* X_k;
@@ -184,17 +189,6 @@ float* P_k;
float* H;
float* H_t;
-int ii, ij;
-
-
-
-
-//uses the global variable j to switch between using 1 and 2
-void KalmanFilter()
- {
-
-
-
if(j == 0)
{
H = &H_1[0][0];
@@ -235,62 +229,53 @@ else
Serial.println("Beginning");
// predict
// predicted state estimate
-math.MatrixMult((float*)F, (float*)X_k_1, n, n, 1, (float*)X_k);
-//math.MatrixMult((float*)B, (float*)u, n, p, 1, (float*)X_k_1);
-math.MatrixCopy((float*)X_k, n, 1, (float*)X_k_1);
-//math.MatrixAdd((float*)temp1, (float*)X_k_1, n, 1, (float*)X_k_1);
-//Serial.println("Estimate");
+math.MatrixMult((float*)F, (float*)X_k_1, n, n, 1, (float*)temp1);
+math.MatrixMult((float*)B, (float*)&u, n, p, 1, (float*)X_k_1);
+//math.MatrixCopy((float*)temp1, n, 1, (float*)X_k_1);
+math.MatrixAdd((float*)temp1, (float*)X_k_1, n, 1, (float*)X_k_1);
+Serial.println("Estimate");
// predicted estimate covariance
math.MatrixMult((float*)F, (float*)P_k_1, n, n, n, (float*)temp2);
-math.MatrixMult((float*)temp2, (float*)F_t, n, n, n, (float*)P_k);
-math.MatrixAdd((float*)P_k, (float*)Q, n, n, (float*)P_k_1);
-//Serial.println("Cov");
+math.MatrixMult((float*)temp2, (float*)F_t, n, n, n, (float*)temp3);
+math.MatrixAdd((float*)temp3, (float*)Q, n, n, (float*)P_k_1);
+Serial.println("Cov");
// update
// innovation or measurement residual
math.MatrixMult((float*)H, (float*)X_k_1, m, n, 1, (float*)temp4);
math.MatrixSubtract((float*)ProcessedSensorData, (float*)temp4, m, 1, (float*)temp4);
-//Serial.println("Res");
+Serial.println("Res");
// innovation or residual covariance
math.MatrixMult((float*)H, (float*)P_k_1, m, n, n, (float*)tempmn);
-//Serial.println("ResCov");
+Serial.println("ResCov");
math.MatrixMult((float*)tempmn, (float*)H_t, m, n, m, (float*)tempm);
-
-
- for(ii = 0; ii < m; ii++){
-
- tempm[ii][ii] = tempm[ii][ii] + R[ii][ii];
-
- }
-//math.MatrixAdd((float*)tempm, (float*)R, m, m, (float*)tempm2);
-
+math.MatrixAdd((float*)tempm, (float*)R, m, m, (float*)tempm);
+Serial.println("end rescov");
// Optimal Kalman gain
math.MatrixInvert((float*)tempm, m);
-//math.MatrixPrint((float*)H_t, n, n, " ");
+Serial.println("KGainstart");
math.MatrixMult((float*)P_k_1, (float*)H_t, n, n, m, (float*)tempnm);
-math.MatrixMult((float*)tempnm, (float*)tempm, n, m, m, (float*)temp5);
-
-//Serial.println("KGain");
+math.MatrixMult((float*)tempnm, (float*)tempm, n, m, m, (float*)tempnm2);
+//math.MatrixMult((float*)P_k_1, (float*)tempnm, n, n, m, (float*)tempnm2);
+//math.MatrixMult((float*)tempnm2, (float*)tempm2, n, m, m, (float*)tempnm);
+Serial.println("KGain");
// updated state estimate
-math.MatrixMult((float*)temp5, (float*)temp4, n, m, 1, (float*)X_k);
+math.MatrixMult((float*)tempnm2, (float*)temp4, n, m, 1, (float*)X_k);
math.MatrixAdd((float*)X_k, (float*)X_k_1, n, 1, (float*)X_k);
-
-//Serial.println("State");
+Serial.println("State");
//updated estimate covarience
-math.MatrixMult((float*)tempnm, (float*)H, n, m, n, (float*)temp2);
+math.MatrixMult((float*)tempnm2, (float*)H, n, m, n, (float*)temp2);
-
- for(ii = 0; ii < n; ii++){
- for(ij = 0; ij < n; ij++){
- temp2[ii][ij] = -temp2[ii][ij];
+int i, j;
+ for(i = 0; i < n; i++){
+ for(j = 0; j < n; j++){
+ temp2[i][j] = -temp2[i][j];
}
}
- for(ii = 0; ii < n; ii++){
- temp2[ii][ii] +=1;
+ for(i = 0; i < n; i++){
+ temp2[i][i] +=1;
}
math.MatrixMult((float*)temp2, (float*)P_k_1, n, n, n, (float*)P_k);
-
-//Serial.println("Ending");
+Serial.println("Ending");
}
-
Oops, something went wrong.

0 comments on commit d8e03a8

Please sign in to comment.