Team Fox
PWM and ALGO updated.. PWM getting generated
Dependencies: FreescaleIAP mbed-rtos mbed
Fork of
TFR_BAE_vr1_1working_finally_PWM_CTRLALGO_update
by 
Team Fox
Diff: TCTM.cpp
- Revision:
- 14:ccdf8790a15e
- Parent:
- 6:036d08b62785
--- a/TCTM.cpp Sat Apr 02 14:27:09 2016 +0000
+++ b/TCTM.cpp Thu Apr 07 18:12:20 2016 +0000
@@ -23,6 +23,11 @@
extern float data[6];
extern float moment[3];
+extern PwmOut PWM1; //x //Functions used to generate PWM signal
+extern PwmOut PWM2; //y
+extern PwmOut PWM3; //z //PWM output comes from pins p6
+
+
extern void FCTN_ATS_DATA_ACQ();
extern void FCTN_ACS_CNTRLALGO(float*,float*);
@@ -217,47 +222,136 @@
{
case 0xE0:
{
+
+ float B[3],W[3];
printf("ACS_COMSN\r\n");
//ACK_L234_TM
+ uint8_t B_x[2];
+ uint8_t B_y[2];
+ uint8_t B_z[2];
+ uint8_t W_x[2];
+ uint8_t W_y[2];
+ uint8_t W_z[2];
+
+ B_x[0]=tc[3];
+ B_x[1]=tc[4];
+ B_y[0]=tc[5];
+ B_y[1]=tc[6];
+ B_z[0]=tc[7];
+ B_z[1]=tc[8];
+
+ W_x[0]=tc[9];
+ W_x[1]=tc[10];
+ W_y[0]=tc[11];
+ W_y[1]=tc[12];
+ W_z[0]=tc[13];
+ W_z[1]=tc[14];
+
+ FCTN_CONVERT_UINT (B_x, &B[0]);
+ FCTN_CONVERT_UINT (B_y, &B[1]);
+ FCTN_CONVERT_UINT (B_z, &B[2]);
+
+ FCTN_CONVERT_UINT (W_x, &W[0]);
+ FCTN_CONVERT_UINT (W_y, &W[1]);
+ FCTN_CONVERT_UINT (W_z, &W[2]);
+
+
+
+
+
tm[0]=0xB0;
tm[1]=tc[0];
tm[2]=ACK_CODE;
- FCTN_ATS_DATA_ACQ(); //get data
+ //FCTN_ATS_DATA_ACQ(); //get data
printf("gyro values\n\r");
for(int i=0; i<3; i++)
- printf("%f\n\r",data[i]);
+ printf("%f\n\r",W[i]);
printf("mag values\n\r");
- for(int i=3; i<6; i++)
- printf("%f\n\r",data[i]);
- FCTN_CONVERT_FLOAT(data[0],&tm[4]); //tm[4] - tm[7]
- FCTN_CONVERT_FLOAT(data[1],&tm[8]); //tm[8] - tm[11]
- FCTN_CONVERT_FLOAT(data[2],&tm[12]); //tm[12] - tm[15]
- FCTN_CONVERT_FLOAT(data[0],&tm[16]); //tm[16] - tm[19]
- FCTN_CONVERT_FLOAT(data[1],&tm[20]); //tm[20] - tm[23]
- FCTN_CONVERT_FLOAT(data[2],&tm[24]); //tm[24] - tm[27]
- if((data[0]<8) && (data[1]<8) && (data[2] <8))
- tm[28] = 1; // gyro values in correct range
- else
- tm[28] = 0;
- if ((data[3] > 20 ) && (data[4] >20) && (data[5]>20)&& (data[3] < 50 ) && (data[4] <50) && (data[5]<50))
- tm[29] = 1; // mag values in correct range
- else
- tm[29] = 0;
+ for(int i=0; i<3; i++)
+ printf("%f\n\r",B[i]);
+ //FCTN_CONVERT_FLOAT(data[0],&tm[4]); //tm[4] - tm[7]
+ //FCTN_CONVERT_FLOAT(data[1],&tm[8]); //tm[8] - tm[11]
+ //FCTN_CONVERT_FLOAT(data[2],&tm[12]); //tm[12] - tm[15]
+ //FCTN_CONVERT_FLOAT(data[0],&tm[16]); //tm[16] - tm[19]
+ //FCTN_CONVERT_FLOAT(data[1],&tm[20]); //tm[20] - tm[23]
+ //FCTN_CONVERT_FLOAT(data[2],&tm[24]); //tm[24] - tm[27]
+ //if((data[0]<8) && (data[1]<8) && (data[2] <8))
+ // tm[28] = 1; // gyro values in correct range
+ //else
+ // tm[28] = 0;
+ //if ((data[3] > 20 ) && (data[4] >20) && (data[5]>20)&& (data[3] < 50 ) && (data[4] <50) && (data[5]<50))
+ // tm[29] = 1; // mag values in correct range
+ //else
+ // tm[29] = 0;
- float B[3],W[3];
- B[0] = B0;
- B[1] = B1;
- B[2] = B2;
- W[0] = W0;
- W[1] = W1;
- W[2] = W2;
+
// Control algo commissioning
FCTN_ACS_CNTRLALGO(B,W);
- FCTN_CONVERT_FLOAT(moment[0],&tm[30]); //tm[30] - tm[33]
- FCTN_CONVERT_FLOAT(moment[1],&tm[34]); //tm[34] - tm[37]
- FCTN_CONVERT_FLOAT(moment[2],&tm[38]); //tm[38] - tm[41]
+ FCTN_CONVERT_FLOAT(moment[0],&tm[4]); //tm[4] - tm[7]
+ FCTN_CONVERT_FLOAT(moment[1],&tm[8]); //tm[8] - tm[11]
+ FCTN_CONVERT_FLOAT(moment[2],&tm[12]); //tm[12] - tm[15]
// to include commission TR as well
- for(uint8_t i=42;i<132;i++)
+ for(uint8_t i=16;i<132;i++)
+ {
+ tm[i]=0x00;
+ }
+
+ crc16 = CRC::crc16_gen(tm,132);
+ tm[133] = (uint8_t)((crc16&0xFF00)>>8);
+ tm[134] = (uint8_t)(crc16&0x00FF);
+ return tm;
+
+ }
+
+
+
+ case 0xE1:
+ {
+ float moment_tc[3];
+ printf("HARDWARE_COMSN\r\n");
+ //ACK_L234_TM
+ uint8_t M0[2];
+ uint8_t M1[2];
+ uint8_t M2[2];
+
+ M0[0]=tc[3];
+ M0[1]=tc[4];
+ M1[0]=tc[5];
+ M1[1]=tc[6];
+ M2[0]=tc[7];
+ M2[1]=tc[8];
+
+
+ tm[0]=0xB0;
+ tm[1]=tc[0];
+ tm[2]=ACK_CODE;
+ FCTN_CONVERT_UINT (M0, &moment_tc[0]);
+ FCTN_CONVERT_UINT (M1, &moment_tc[1]);
+ FCTN_CONVERT_UINT (M2, &moment_tc[2]);
+
+
+
+ FCTN_ACS_GENPWM_MAIN(moment_tc);
+
+ float PWM_measured[3];
+
+ PWM_measured[0] = PWM1.read();
+ PWM_measured[1] = PWM2.read();
+ PWM_measured[2] = PWM3.read();
+
+
+ FCTN_CONVERT_FLOAT(PWM_measured[0],&tm[4]); //4-7
+ FCTN_CONVERT_FLOAT(PWM_measured[1],&tm[8]); //8-11
+ FCTN_CONVERT_FLOAT(PWM_measured[2],&tm[12]); //12-15
+ for(int i=0; i<12; i++)
+ FCTN_CONVERT_FLOAT(actual_data.current_actual[i],&tm[16 + (i*4)]);
+
+
+ FCTN_ATS_DATA_ACQ(); //get data
+
+
+ // to include commission TR as well
+ for(uint8_t i=64;i<132;i++)
{
tm[i]=0x00;
}
@@ -784,9 +878,17 @@
std::cout << "\n\r uint32"<<*temp << std::endl;
output[0] =(uint8_t )(((*temp)>>24)&0xFF);
- output[2] =(uint8_t ) (((*temp)>>16)&0xFF);
- output[1] =(uint8_t ) (((*temp)>>8)&0xFF);
+ output[1] =(uint8_t ) (((*temp)>>16)&0xFF);
+ output[2] =(uint8_t ) (((*temp)>>8)&0xFF);
output[3] =(uint8_t ) ((*temp) & 0xFF); // verify the logic
//printf("\n\r inside %d %d %d %d", output[3],output[2],output[1],output[0]);
//std:: cout << "\n\r uint8 inside " << output[3] << '\t' << output[2] << '\t' << output[1] << '\t' << output[0] <<std::endl;
+}
+
+void FCTN_CONVERT_UINT (uint8_t input[2], float* output)
+{
+
+ *output = (float) input[1];
+ *output = *output/100.0; //input[0] integer part
+ *output = *output + (float) input[0]; //input[1] decimal part correct to two decimal places
}
\ No newline at end of file