LDSC_Robotics_TAs
Version_1
Revision 1:235bfa26c5c1, committed 2016-09-05
- Comitter:
- noname001
- Date:
- Mon Sep 05 02:02:19 2016 +0000
- Parent:
- 0:0b8d14ed499f
- Commit message:
- Version_2 (Date:2016-09-05)
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Wed Aug 31 08:55:00 2016 +0000
+++ b/main.cpp Mon Sep 05 02:02:19 2016 +0000
@@ -1,52 +1,56 @@
+// 導入需要之header檔
+//#include <p30f4011.h>
#include "mbed.h"
#include <math.h>
#include "LSM9DS0.h"
-//#include "can.h"
-Ticker timer1;
-
-Serial uart_1(D10,D2); // TX : D10 RX : D2 // serial 1
-#define LOOPTIME 500 // 2 ms
-unsigned long lastMilli = 0;
-// sampling time
-float T = 0.002;
-Timer t;
-
+Ticker timer1; // 2 ms
+Serial uart_1(D10,D2); // TX : D10 RX : D2 // serial 1
+LSM9DS0 sensor(SPI_MODE, D7, D6); // SPI_CS1 : D7 , SPI_CS2 : D6
+AnalogIn adc_switch(A0);
// uart data
int display[6] = {0,0,0,0,0,0};
-char num[14]= {254,255,0,0,0,0,0,0,0,0,0,0,0,0}; // char num[0] , num[1] : 2 start byte
+char num[14]={254,255,0,0,0,0,0,0,0,0,0,0,0,0}; // char num[0] , num[1] : 2 start byte
+// 等待sensor上電
int tcount = 0;
int init_flag = 0;
+int flag_d_1;
+int flag_d_2;
+int flag_d_3;
+int flag_d_4;
+int flag_d_5;
+int flag_d_6;
int i;
int j;
int k;
-
+int z;
+float T = 0.002;
// sensor gain , 由實驗測得
-#define Gyro_gain_x 0.001223735681818
-#define Gyro_gain_y 0.001223735681818
-#define Gyro_gain_z 0.001223735681818
-#define Acce_gainx -0.002386188420346 // -9.81 / ( 3264.287 - (-846.872) )
+//#define Gyro_gain_x 0.001223735681818
+#define Gyro_gain_x 0.001223735681818f
+#define Gyro_gain_y 0.001223735681818f
+#define Gyro_gain_z 0.001223735681818f
+#define Acce_gainx -0.002386188420346f // -9.81 / ( 3264.287 - (-846.872) )
//#define Acce_gainy -0.002372964173321 // -9.81 / ( 3508 - (-626.07) )
-#define Acce_gainy -0.002512964173321
-#define Acce_gainz -0.002381154075718 // -9.81 / ( 4413.257 - (293.406) )
-
-
+#define Acce_gainy -0.002512964173321f
+#define Acce_gainz -0.002381154075718f // -9.81 / ( 4413.257 - (293.406) )
// 宣告從sensor讀到的值要存入處理的變數
-
+int16_t Gyro_axis_data[3]; // X, Y, Z axis
float Gyro_axis_data_f[3];
float Gyro_axis_data_f_old[3];
float Gyro_scale[3]; // scale = (data - zero) * gain
-float Gyro_axis_zero[3] = {0,-8,8};
+float Gyro_axis_zero[3] = {0,-8,8};
+int16_t Acce_axis_data[3]; // X, Y, Z axis
float Acce_axis_data_f[3];
float Acce_axis_data_f_old[3];
float Acce_scale[3]; // scale = (data - zero) * gain
//float Acce_axis_zero[3] = {-846.872,-626.07,293.406};
-float Acce_axis_zero[3] = {-907,-658,225};
+float Acce_axis_zero[3] = {-907,-658,225};
// final sensor output value
float axm = 0; // Acce_scale
@@ -57,7 +61,7 @@
float u2 = 0; // Gyro_scale
float u3 = 0; // Gyro_scale
-// counter for gyro bias
+// counter for gyro bias //improve
int gyro_count = 0;
int gyro_flag = 0;
float gyro_init_1[10] = {0,0,0,0,0,0,0,0,0,0};
@@ -69,21 +73,20 @@
float gyro_sum_3 = 0;
// new defined direction
-float ax = 0;
-float ay = 0;
+float ax = 0;
+float ay = 0;
float az = 0;
-float w1 = 0;
-float w2 = 0;
+float w1 = 0;
+float w2 = 0;
float w3 = 0;
-float w1_d = 0;
-float w2_d = 0;
+float w1_d = 0;
+float w2_d = 0;
float w3_d = 0;
-// sampling time
-//float T = 0.002; // 2ms -> 500Hz
-char uart_read = 0;
+//read weight
+//char uart_read = 0;
// 速度 , 向心加速度
int rotationspeed = 0;
@@ -131,7 +134,7 @@
float theta_deg = 0;
// normalized variables
-float n = 0;
+float n = 0;
float gs1_hat_n = 0;
float gs2_hat_n = 0;
float gs3_hat_n = 0;
@@ -155,6 +158,7 @@
int compensationrate = 0;
int Mb = 25; // bicycle mass
int M = 95; // total mass (25+70)
+int adc = 0;
int buttom_buf = 0;
int buttom_buf_old = 0;
// 開關安全措施
@@ -164,13 +168,25 @@
int open_flag = 0;
int protect_flag = 0;
-void timer1_interrupt(void);
-void init_TIMER(void);
+
+// 函式宣告
void init_uart(void);
-void uart_send(void);
void separate(void);
void init_Sensors(void);
+void init_TIMER();
+void timer1_interrupt(void); // void _ISR _T1Interrupt(void);
void read_sensor(void);
+
+//unsigned char SPI_operation(unsigned char data_send);
+//void sensorG_setup(void);
+//void sensorXM_setup(void);
+//void sensorG_read_3axis(void);
+//void sensorXM_read_3axis(void);
+//void init_ADC(void);
+//void _ISR _U2RXInterrupt(void);
+//void init_IO(void);
+// void init_spi(void);
+
float lpf(float input,float output_old,float frequency);
float sinr(float input);
float cosr(float input);
@@ -178,60 +194,436 @@
float arccos(float input);
float arctan(float input);
-LSM9DS0 sensor(SPI_MODE, D7, D6); // SPI_CS1 : D7 , SPI_CS2 : D6
-// sensor data
-int16_t Gyro_axis_data[3] = {0}; // X, Y, Z axis
-int16_t Acce_axis_data[3] = {0}; // X, Y, Z axis
-int16_t Magn_axis_data[3] = {0}; // X, Y, Z axis
-
-int main()
-{
-
+// Main routine
+int main(void)
+{
//init_IO();
- init_TIMER(); //t.strat();
+//init_spi();
+ //init_ADC();
+ init_TIMER();
init_uart();
- //init_spi();
- //init_can();
- //init_ADC();
- // t.start();
+
+ while(1) // 永久迴圈
+ {
+
+ }
+
+} // End of main()
- while(1) {
-
- }
+/*
+void init_IO(void)
+{
+ // 1為數位，0為類比
+ ADPCFG = 0xffff;
+
+ //LED
+ TRISDbits.TRISD1 = 0; // 定義I/O腳位輸入輸出 0:output 1:input
+ TRISDbits.TRISD2 = 0;
+ TRISDbits.TRISD3 = 0;
+ LATDbits.LATD1 = 1; // 1:dark 0:light
+ LATDbits.LATD2 = 1;
+ LATDbits.LATD3 = 1;
+
+ //SPI CS
+ TRISBbits.TRISB4 = 0; // CSXM
+ TRISBbits.TRISB5 = 0; // CSG
+ LATBbits.LATB4 = 1;
+ LATBbits.LATB5 = 1;
+
+ //ADC
+ ADPCFGbits.PCFG6 = 1; // AN6 = digital
+ TRISBbits.TRISB6 = 1; // input 讀取電壓
+}
+*/
+void init_TIMER(){
+ i = 0;
+ timer1.attach_us(&timer1_interrupt, 2000.0); // 2 ms
}
-void init_uart()
-{
- uart_1.baud(115200); // 設定baudrate
-}
-
-
-void uart_send(void) //uart_send() neen initial i (i=0)
+//void _ISR1 _T1Interrupt(void) // 500Hz
+void timer1_interrupt(void)
{
- // uart send data
- display[0] = Gyro_axis_data[0];
- display[1] = Gyro_axis_data[1];
- display[2] = Gyro_axis_data[2];
- display[3] = Acce_axis_data[0];
- display[4] = Acce_axis_data[1];
- display[5] = Acce_axis_data[2];
+ //LATDbits.LATD3 = 1;
+
+ if(init_flag == 0) //用timer代替 數超過300次 開始讀植
+ {
+ tcount = tcount + 1;
+
+ if(tcount >= 300)
+ {
+ init_Sensors(); // 讓sensor完全上電後再初始化sensor
+ tcount = 0;
+ init_flag = 1;
+ }
+ }
+
+ else
+ {
+ read_sensor();
+
+ // gyro filter
+ Gyro_axis_data_f[0] = lpf(Gyro_axis_data[0],Gyro_axis_data_f_old[0],18);
+ Gyro_axis_data_f_old[0] = Gyro_axis_data_f[0];
+ Gyro_axis_data_f[1] = lpf(Gyro_axis_data[1],Gyro_axis_data_f_old[1],18);
+ Gyro_axis_data_f_old[1] = Gyro_axis_data_f[1];
+ Gyro_axis_data_f[2] = lpf(Gyro_axis_data[2],Gyro_axis_data_f_old[2],18);
+ Gyro_axis_data_f_old[2] = Gyro_axis_data_f[2];
+
+ // gyro bias average
+ if(gyro_flag == 0)
+ {
+ gyro_count = gyro_count + 1;
+
+ if(gyro_count == 500)
+ {
+ gyro_init_1[index_count] = Gyro_axis_data_f[0];
+ gyro_init_2[index_count] = Gyro_axis_data_f[1];
+ gyro_init_3[index_count] = Gyro_axis_data_f[2];
+ index_count++;
+ }
+ else if(gyro_count == 505)
+ {
+ gyro_init_1[index_count] = Gyro_axis_data_f[0];
+ gyro_init_2[index_count] = Gyro_axis_data_f[1];
+ gyro_init_3[index_count] = Gyro_axis_data_f[2];
+ index_count++;
+ }
+ else if(gyro_count == 510)
+ {
+ gyro_init_1[index_count] = Gyro_axis_data_f[0];
+ gyro_init_2[index_count] = Gyro_axis_data_f[1];
+ gyro_init_3[index_count] = Gyro_axis_data_f[2];
+ index_count++;
+ }
+ else if(gyro_count == 520)
+ {
+ gyro_init_1[index_count] = Gyro_axis_data_f[0];
+ gyro_init_2[index_count] = Gyro_axis_data_f[1];
+ gyro_init_3[index_count] = Gyro_axis_data_f[2];
+ index_count++;
+ }
+ else if(gyro_count == 530)
+ {
+ gyro_init_1[index_count] = Gyro_axis_data_f[0];
+ gyro_init_2[index_count] = Gyro_axis_data_f[1];
+ gyro_init_3[index_count] = Gyro_axis_data_f[2];
+ index_count++;
+ }
+ else if(gyro_count == 550)
+ {
+ gyro_init_1[index_count] = Gyro_axis_data_f[0];
+ gyro_init_2[index_count] = Gyro_axis_data_f[1];
+ gyro_init_3[index_count] = Gyro_axis_data_f[2];
+ index_count++;
+ }
+ else if(gyro_count == 560)
+ {
+ gyro_init_1[index_count] = Gyro_axis_data_f[0];
+ gyro_init_2[index_count] = Gyro_axis_data_f[1];
+ gyro_init_3[index_count] = Gyro_axis_data_f[2];
+ index_count++;
+ }
+ else if(gyro_count == 570)
+ {
+ gyro_init_1[index_count] = Gyro_axis_data_f[0];
+ gyro_init_2[index_count] = Gyro_axis_data_f[1];
+ gyro_init_3[index_count] = Gyro_axis_data_f[2];
+ index_count++;
+ }
+ else if(gyro_count == 580)
+ {
+ gyro_init_1[index_count] = Gyro_axis_data_f[0];
+ gyro_init_2[index_count] = Gyro_axis_data_f[1];
+ gyro_init_3[index_count] = Gyro_axis_data_f[2];
+ index_count++;
+ }
+ else if(gyro_count >= 590)
+ {
+ gyro_init_1[index_count] = Gyro_axis_data_f[0];
+ gyro_init_2[index_count] = Gyro_axis_data_f[1];
+ gyro_init_3[index_count] = Gyro_axis_data_f[2];
+ z = 0;
+ for(z=0 ; z<10 ; z++)
+ {
+ gyro_sum_1 = gyro_sum_1 + gyro_init_1[z];
+ gyro_sum_2 = gyro_sum_2 + gyro_init_2[z];
+ gyro_sum_3 = gyro_sum_3 + gyro_init_3[z];
+ }
+
+ Gyro_axis_zero[0] = gyro_sum_1 / 10;
+ Gyro_axis_zero[1] = gyro_sum_2 / 10;
+ Gyro_axis_zero[2] = gyro_sum_3 / 10;
+ index_count = 0;
+ gyro_count = 0;
+ gyro_flag = 1;
+ }
+ }
+ //831 2:20
+ // acce filter
+ Acce_axis_data_f[0] = lpf(Acce_axis_data[0],Acce_axis_data_f_old[0],18);
+ Acce_axis_data_f_old[0] = Acce_axis_data_f[0];
+ Acce_axis_data_f[1] = lpf(Acce_axis_data[1],Acce_axis_data_f_old[1],18);
+ Acce_axis_data_f_old[1] = Acce_axis_data_f[1];
+ Acce_axis_data_f[2] = lpf(Acce_axis_data[2],Acce_axis_data_f_old[2],18);
+ Acce_axis_data_f_old[2] = Acce_axis_data_f[2];
+
+ Gyro_scale[0] = (Gyro_axis_data_f[0]-Gyro_axis_zero[0])*Gyro_gain_x; //88888
+ Gyro_scale[1] = (Gyro_axis_data_f[1]-Gyro_axis_zero[1])*Gyro_gain_y;
+ Gyro_scale[2] = (Gyro_axis_data_f[2]-Gyro_axis_zero[2])*Gyro_gain_z;
+
+ Acce_scale[0] = ((Acce_axis_data_f[0]-Acce_axis_zero[0]))*Acce_gainx;
+ Acce_scale[1] = ((Acce_axis_data_f[1]-Acce_axis_zero[1]))*Acce_gainy;
+ Acce_scale[2] = ((Acce_axis_data_f[2]-Acce_axis_zero[2]))*Acce_gainz;
+
+ // final 6 axis data
+ axm = Acce_scale[0];
+ aym = Acce_scale[1];
+ azm = Acce_scale[2];
+
+ u1 = Gyro_scale[0];
+ u2 = Gyro_scale[1];
+ u3 = Gyro_scale[2];
+
+ // new defined direction
+ ax = -aym;
+ ay = axm;
+ az = azm;
+
+ w1 = -u2;
+ w2 = u1;
+ w3 = u3;
+
+ // w deadzone
+ flag_d_1 = 0;
+ flag_d_2 = 0;
+ flag_d_1 = w1 <= 0.015f;
+ flag_d_2 = w1 >= -0.015f;
+ if((flag_d_1 + flag_d_2) == 2)
+ w1_d = 0;
+ else
+ w1_d = w1;
+
+ flag_d_3 = 0;
+ flag_d_4 = 0;
+ flag_d_3 = w2 <= 0.015f;
+ flag_d_4 = w2 >= -0.015f;
+ if((flag_d_3 + flag_d_4) == 2)
+ w2_d = 0;
+ else
+ w2_d = w2;
+
+ flag_d_5 = 0;
+ flag_d_6 = 0;
+ flag_d_5 = w3 <= 0.015f;
+ flag_d_6 = w3 >= -0.015f;
+ if((flag_d_5 + flag_d_6) == 2)
+ w3_d = 0;
+ else
+ w3_d = w3;
+ }
+
+ // velocity filter
+ v_f = lpf(v,v_f_old,18);
+ v_f_old = v_f;
+
+ // 離心加速度
+ acy_hat = 1.4f*v_f*w1_d; // 1.2
+ acy_hat_f = lpf(acy_hat,acy_hat_f_old,18);
+ acy_hat_f_old = acy_hat_f;
+
+ // sensor fusion by backward Euler method
+ gs1_hat = (gs1_hat_old + T*(-w2_d*alpf_hat + p*ax - w3_d*(acy_hat_f-ay) - w2_d*az)) / (1+p*T);
+ gs1_hat_old = gs1_hat;
+
+ gs2_hat = (gs2_hat_old + T*(w1_d*alpf_hat - w3_d*ax - p*(acy_hat_f-ay) + w1_d*az)) / (1+p*T);
+ gs2_hat_old = gs2_hat;
+
+ gs3_hat = (gs3_hat_old + T*(-b*alpf_hat + c*v_hat + w2_d*ax + w1_d*(acy_hat_f-ay) - b*az - c*v_f)) / (1-b*T);
+ gs3_hat_old = gs3_hat;
+
+ alpf_hat = (alpf_hat_old + T*((a+d)*gs3_hat + e*v_hat - (a+d)*az - e*v_f)) / (1+(a+d)*T);
+ alpf_hat_old = alpf_hat;
+
+ v_hat = (v_hat_old + T*((1+f)*gs3_hat - f*alpf_hat - (1+f)*az - g*v_f)) / (1-g*T);
+ v_hat_old = v_hat;
+
+ /*
+ // 離心加速度
+ acy_hat = 1.2*v_f*w1;
+ acy_hat_f = lpf(acy_hat,acy_hat_f_old,18);
+ acy_hat_f_old = acy_hat_f;
+
+ // sensor fusion by backward Euler method
+ gs1_hat = (gs1_hat_old + T*(-w2*alpf_hat + p*ax - w3*(acy_hat_f-ay) - w2*az)) / (1+p*T);
+ gs1_hat_old = gs1_hat;
+
+ gs2_hat = (gs2_hat_old + T*(w1*alpf_hat - w3*ax - p*(acy_hat_f-ay) + w1*az)) / (1+p*T);
+ gs2_hat_old = gs2_hat;
+
+ gs3_hat = (gs3_hat_old + T*(-b*alpf_hat + c*v_hat + w2*ax + w1*(acy_hat_f-ay) - b*az - c*v_f)) / (1-b*T);
+ gs3_hat_old = gs3_hat;
+
+ alpf_hat = (alpf_hat_old + T*((a+d)*gs3_hat + e*v_hat - (a+d)*az - e*v_f)) / (1+(a+d)*T);
+ alpf_hat_old = alpf_hat;
+
+ v_hat = (v_hat_old + T*((1+f)*gs3_hat - f*alpf_hat - (1+f)*az - g*v_f)) / (1-g*T);
+ v_hat_old = v_hat;
+ */
+
+ // normalize gs1_hat , gs2_hat , gs3_hat to find pitch angle and roll angle
+ n = sqrt(gs1_hat*gs1_hat + gs2_hat*gs2_hat + gs3_hat*gs3_hat);
+
+ gs1_hat_n = (gs1_hat / n) * 9.81f;
+ gs2_hat_n = (gs2_hat / n) * 9.81f;
+ gs3_hat_n = (gs3_hat / n) * 9.81f;
+
+ psi = arcsin(-gs3_hat_n / 9.81f);
+ //theta = atan(-gs2_hat_n / gs1_hat_n);
+
+ // psi correction
+ psi_m = psi - (0); // after correction
+ psi_deg = 57.3f * psi_m;
+
+ // psi deadzone
+ flag_d_1 = 0;
+ flag_d_2 = 0;
+ flag_d_1 = psi_deg < 2.5f; // 在平地時 flag_d_1 = 1
+ flag_d_2 = psi_deg > -3; // 在平地時 flag_d_2 = 1
+
+ if((flag_d_1 + flag_d_2) == 2) // 在平地時 flag_d_1 + flag_d_2 = 2
+ psi_d = 0;
+ else
+ psi_d = psi_m;
+ // 去除負加速度
+ if(alpf_hat < 0)
+ alpf_c = 0;
+ else
+ alpf_c = alpf_hat;
+
+ // 速度為負時不考慮加速度
+ if(v_f < 0)
+ alpf_v = 0;
+ else
+ alpf_v = alpf_c;
+
+ // 加速度 deadzone
+ if(alpf_v <= 0.1f)
+ alpf_d = 0;
+ else
+ alpf_d = alpf_v;
+
+ // 改button //improve //設定一個開關
+ /*
+ // buttom filter
+ buttom_buf = lpf(ADCBUF0,buttom_buf_old,30);
+ buttom_buf_old = buttom_buf;
+
+ // button control compensationrate
+ if(buttom_buf >= 450)
+ compensationrate = 1;
+ else
+ compensationrate = 0;
+ */
+
+ if(adc_switch.read() > 0.6f)
+ {
+ buttom_buf=1;
+ compensationrate = 1;
+ }
+ else
+ {
+ buttom_buf=0;
+ compensationrate = 0;
+ }
+
+ // 電流命令 (T = k*i)
+ current_command = compensationrate * (Mb*alpf_d*1.1f + M*9.81f*sinr(psi_d)*1.35f + Df) * (R/Ki);
+
+ // 電流命令上下限
+ if(current_command >= 15)
+ current_command = 15;
+ else if(current_command <= -25)
+ current_command = -25;
+ //831 412
+ // 開關安全措施
+ b_count = b_count + 1;
+
+ if(b_count >= 1500) // wait for buttom ADC buffer ready
+ {
+ b_init_flag = 1;
+ b_count = 0;
+ }
+
+ if(b_init_flag == 0)
+ open_flag = 1;
+ else if(b_init_flag == 1)
+ {
+ if(compensationrate == 1 && open_flag == 1) // 初始 open_flag = 1
+ protect_flag = 1;
+
+ if(compensationrate == 0) // 初始 open_flag = 1 , 等 compensationrate = 0 時才將 open_flag = 0
+ {
+ protect_flag = 0; // 初始 compensationrate = 0 時才將 protect_flag = 0
+ open_flag = 0;
+ b_flag = 0;
+ }
+ // 要先有初始 compensationrate = 0 才能先將 protect_flag == 0 && open_flag == 0 成立
+ // 之後由 compensationrate = 0 轉變成 compensationrate = 1 時才將 b_flag = 1(開始送命令)
+ if(compensationrate == 1 && protect_flag == 0 && open_flag == 0)
+ {
+ b_flag = 1;
+ }
+ }
+
+ // 改CAN
+ // can 傳資料到大板子
+ //C1TX0DLCbits.DLC = 8;
+ /*
+ if(b_flag == 1)
+ {
+ C1TX0B1 = current_command;
+ C1TX0B2 = compensationrate;
+ C1TX0B3 = 300;
+ }
+ else if(b_flag == 0)
+ {
+ C1TX0B1 = 0;
+ C1TX0B2 = 0;
+ C1TX0B3 = 300;
+ }
+ */
+ //C1TX0CONbits.TXREQ = 1;
+
+ // uart傳送資料
+
+ display[0] = 100*psi_deg;
+ display[1] = 100*gs2_hat;
+ display[2] = 100*gs3_hat;
+ display[3] = buttom_buf;
+ display[4] = current_command;
+ display[5] = rotationspeed;
+
+
separate();
-
+
j = 0;
k = 1;
- while(k) {
- if(uart_1.writeable()) {
+ while(k)
+ {
+ if(uart_1.writeable())
+ {
uart_1.putc(num[i]);
i++;
j++;
}
- if(j>1) { // send 2 bytes
+ if(j>1) // send 2 bytes
+ {
k = 0;
j = 0;
}
@@ -239,7 +631,449 @@
if(i>13)
i = 0;
+
+ //LATDbits.LATD3 = 0;
+
+ //IFS0bits.T1IF = 0; // 重置中斷旗標
}
+
+void init_uart()
+{
+ uart_1.baud(115200); // 設定baudrate
+}
+
+void separate(void)
+{
+ num[2] = display[0] >> 8; // HSB(8bit)資料存入陣列
+ num[3] = display[0] & 0x00ff; // LSB(8bit)資料存入陣列
+ num[4] = display[1] >> 8;
+ num[5] = display[1] & 0x00ff;
+ num[6] = display[2] >> 8;
+ num[7] = display[2] & 0x00ff;
+ num[8] = display[3] >> 8;
+ num[9] = display[3] & 0x00ff;
+ num[10] = display[4] >> 8;
+ num[11] = display[4] & 0x00ff;
+ num[12] = display[5] >> 8;
+ num[13] = display[5] & 0x00ff;
+}
+
+/*
+void init_spi(void)
+{
+ SPI1STATbits.SPISIDL = 1; // Stop in Idle Mode bit
+ SPI1CONbits.FRMEN = 0; // Framed SPI support disabled
+ SPI1CONbits.DISSDO = 0; // SDO1 pin is controlled by the module
+ SPI1CONbits.MODE16 = 0; // Communication is byte-wide (8 bits)
+ SPI1CONbits.SMP = 0; // Input data sampled at middle of data output time
+ SPI1CONbits.CKE = 0; // Serial output data changes on transition from Idle clock state to active clock state
+ SPI1CONbits.CKP = 1; // Idle state for clock is a high level; active state is a low level
+ SPI1CONbits.SSEN = 0; // Slave Select Unable
+ SPI1CONbits.MSTEN = 1; // Master mode
+ // Fsck = Fcy / (Secondary Prescale * Primary Prescale)
+ SPI1CONbits.SPRE = 0b111; // Secondary Prescale (Master Mode) bits , 111 = Secondary prescale 1:1
+ SPI1CONbits.PPRE = 0b01; // Primary Prescale (Master Mode) bits , 01 = Primary prescale 16:1
+
+ // SPI enable 最後才能打開
+ SPI1STATbits.SPIEN = 1; // Enables module and configures SCK1, SDO1, SDI1 and SS1 as serial port pins
+}
+*/
+
+void init_Sensors(void)
+{
+ sensor.begin();
+// sensor.begin() setting :
+ // uint16_t begin(gyro_scale gScl = G_SCALE_2000DPS,
+ // accel_scale aScl = A_SCALE_8G,
+ // mag_scale mScl = M_SCALE_8GS,
+ // gyro_odr gODR = G_ODR_760_BW_100,
+ // accel_odr aODR = A_ODR_800,
+ // mag_odr mODR = M_ODR_100);
+
+//sensor_CSXM = 1;
+ //sensor_CSG = 1;
+ //Gyro_axis_data[0] = 0; // X
+ //Gyro_axis_data[1] = 0; // Y
+ //Gyro_axis_data[2] = 0; // Z
+ //Acce_axis_data[0] = 0;
+ //Acce_axis_data[1] = 0;
+ //Acce_axis_data[2] = 0;
+ //sensorG_setup();
+ //sensorXM_setup();
+}
+/*
+void init_sensor(void)
+{
+ sensor.begin();
+ // sensor.begin() setting :
+ // uint16_t begin(gyro_scale gScl = G_SCALE_2000DPS,
+ // accel_scale aScl = A_SCALE_8G,
+ // mag_scale mScl = M_SCALE_8GS,
+ // gyro_odr gODR = G_ODR_760_BW_100,
+ // accel_odr aODR = A_ODR_800,
+ // mag_odr mODR = M_ODR_100);
+}
+*/
+/*
+unsigned char SPI_operation(unsigned char data_send) // 具有 write 跟 read 兩種功能
+{
+ static unsigned char data_recived = 0;
+
+ while(SPI1STATbits.SPITBF==1); // wait for SPI1STATbits.SPITBF = 0 (transmit start , SPI1TXB is empty)
+ SPI1BUF = data_send; // write SPI
+
+ while(SPI1STATbits.SPIRBF==0); // wait for SPI1STATbits.SPIRBF = 1 (receive complete , SPI1RXB is full)
+ data_recived = SPI1BUF; // read SPI
+
+ return data_recived;
+}
+*/
+/*
+void sensorG_setup(void)
+{
+ static unsigned char sensorG_data_write;
+ static unsigned char sensorG_data_read;
+
+ // 設定暫存器的值
+ sensorG_CTRL_REG1 = 0b11111111; // ODR = 760Hz , BW = 100Hz
+ // Power-down mode is normal mode , XYZ enable
+ sensorG_CTRL_REG2 = 0b00001001; // High-pass filter mode is normal mode
+ // High-pass filter cutoff frequency = 0.09Hz
+ sensorG_CTRL_REG3 = 0b00000000; // no use , all 0
+ sensorG_CTRL_REG4 = 0b00110000; // 2000dps
+ sensorG_CTRL_REG5 = 0b00000000; // no use , all 0
+
+ // 設定暫存器的位址
+ // write mode (第8個bit = 0)
+ // auto increase address (第7個bit = 1)
+ sensorG_data_write = 0x20; // sensor_G_CTRL address
+ sensorG_data_write = (sensorG_data_write & 0b00111111) | 0b01000000;
+ // mask first two bits : sensorG_CTRL_REG1_address & 0b00111111 , 把最前面兩個bit清0 , 其他bit保留不動
+ // write first two bits : (...) | 0b01000000 把最前面兩個bit寫成01 , 其他bit保留不動
+
+ // open
+ sensor_CSG = 0;
+
+ // CS打開後 , 會先寫暫存器的位址 , 再寫暫存器的值
+ // 寫入暫存器的位址後就完成write mode和auto increase address的功能
+ // 接下來就會依序遞增暫存器的位址並做寫入的動作
+ // start to write register address
+ sensorG_data_read = SPI_operation(sensorG_data_write);
+
+ // start to set register value
+ sensorG_data_read = SPI_operation(sensorG_CTRL_REG1);
+ sensorG_data_read = SPI_operation(sensorG_CTRL_REG2);
+ sensorG_data_read = SPI_operation(sensorG_CTRL_REG3);
+ sensorG_data_read = SPI_operation(sensorG_CTRL_REG4);
+ sensorG_data_read = SPI_operation(sensorG_CTRL_REG5);
+
+ // close
+ sensor_CSG = 1;
+}
+*/
+/*
+void sensorXM_setup(void)
+{
+ static unsigned char sensorXM_data_write;
+ static unsigned char sensorXM_data_read;
+
+ // 設定暫存器的值
+ sensorXM_CTRL_REG0 = 0b00000000; // no use , all 0
+ sensorXM_CTRL_REG1 = 0b10010111; // ODR = 800Hz , XYZ enable
+ sensorXM_CTRL_REG2 = 0b11011000; // anti-alias filter BW = 50Hz
+ // acceleration full scale = +-8 g
+ sensorXM_CTRL_REG3 = 0b00000000; // no use , all 0
+ sensorXM_CTRL_REG4 = 0b00000000; // no use , all 0
+ sensorXM_CTRL_REG5 = 0b01110100; // magnetic resolution is high resolution
+ // Power mode selection = 100Hz
+ sensorXM_CTRL_REG6 = 0b01000000; // Magnetic full scale = +-8 gauss
+ sensorXM_CTRL_REG7 = 0b00000000; // no use , all 0
+
+ // 設定暫存器的位址
+ // write mode (第8個bit = 0)
+ // auto increase address (第7個bit = 1)
+ sensorXM_data_write = 0x1F; // sensor_XM_CTRL address
+ sensorXM_data_write = (sensorXM_data_write & 0b00111111) | 0b01000000;
+ // mask first two bits , write first two bits
+
+ // open
+ sensor_CSXM = 0;
+
+ // start to write register address
+ sensorXM_data_read = SPI_operation(sensorXM_data_write);
+
+ // start to set register value
+ sensorXM_data_read = SPI_operation(sensorXM_CTRL_REG0);
+ sensorXM_data_read = SPI_operation(sensorXM_CTRL_REG1);
+ sensorXM_data_read = SPI_operation(sensorXM_CTRL_REG2);
+ sensorXM_data_read = SPI_operation(sensorXM_CTRL_REG3);
+ sensorXM_data_read = SPI_operation(sensorXM_CTRL_REG4);
+ sensorXM_data_read = SPI_operation(sensorXM_CTRL_REG5);
+ sensorXM_data_read = SPI_operation(sensorXM_CTRL_REG6);
+ sensorXM_data_read = SPI_operation(sensorXM_CTRL_REG7);
+
+ // close
+ sensor_CSXM = 1;
+}
+*/
+
+/*
+void sensorG_read_3axis(void)
+{
+ static unsigned char sensorG_data_write;
+ static int sensorG_data_read[6];
+
+ // 設定暫存器的位址
+ // read mode (第8個bit = 1)
+ // auto increase address (第7個bit = 1)
+ sensorG_data_write = 0x28; // gyroscope's address
+ sensorG_data_write = (sensorG_data_write & 0b00111111) | 0b11000000;
+ // mask first two bits , write first two bits
+ // 因為暫存器位址只有6個bit , 所以用mask和write來設定read mode , auto increase address
+ // 不會改變到暫存器位址的值
+
+ // open
+ sensor_CSG = 0;
+
+ // CS打開後 , 會先寫暫存器的位址 , 再寫暫存器的值
+ // 寫入暫存器的位址後就完成read mode和auto increase address的功能
+ // 接下來就會依序遞增暫存器的位址並做讀取的動作
+ // start to write register address
+ sensorG_data_read[0] = (int)SPI_operation(sensorG_data_write);
+
+ // start to read register value
+ // gyroscope output
+ // 因為這邊的SPI函數是用來讀值 , 而SPI函數是把寫和讀功能寫在一起的函數
+ // 所以寫入的值就任意設成0x00 , 反正寫入的值不重要
+ sensorG_data_read[0] = (int)SPI_operation(0x00); // XL
+ sensorG_data_read[1] = (int)SPI_operation(0x00); // XH
+ sensorG_data_read[2] = (int)SPI_operation(0x00); // YL
+ sensorG_data_read[3] = (int)SPI_operation(0x00); // YH
+ sensorG_data_read[4] = (int)SPI_operation(0x00); // ZL
+ sensorG_data_read[5] = (int)SPI_operation(0x00); // ZH
+
+ // close
+ sensor_CSG = 1;
+
+ // data reconstruction
+ Gyro_axis_data[0] = (sensorG_data_read[1]<<8) | sensorG_data_read[0]; // X
+ Gyro_axis_data[1] = (sensorG_data_read[3]<<8) | sensorG_data_read[2]; // Y
+ Gyro_axis_data[2] = (sensorG_data_read[5]<<8) | sensorG_data_read[4]; // Z
+}
+*/
+/*
+void sensorXM_read_3axis(void)
+{
+ static unsigned char sensorXM_data_write;
+ static int sensorXM_data_read[6];
+
+ // 設定暫存器的位址
+ // read mode (第8個bit = 1)
+ // auto increase address (第7個bit = 1)
+ sensorXM_data_write = 0x28; // accelerometer's address
+ sensorXM_data_write = (sensorXM_data_write & 0b00111111) | 0b11000000;
+ // mask first two bits , write first two bits
+
+ // open
+ sensor_CSXM = 0;
+
+ // start to write register address
+ sensorXM_data_read[0] = (int)SPI_operation(sensorXM_data_write);
+
+ // start to read register value
+ // accelerometer output
+ sensorXM_data_read[0] = (int)SPI_operation(0x00); // XL
+ sensorXM_data_read[1] = (int)SPI_operation(0x00); // XH
+ sensorXM_data_read[2] = (int)SPI_operation(0x00); // YL
+ sensorXM_data_read[3] = (int)SPI_operation(0x00); // YH
+ sensorXM_data_read[4] = (int)SPI_operation(0x00); // ZL
+ sensorXM_data_read[5] = (int)SPI_operation(0x00); // ZH
+
+ // close
+ sensor_CSXM = 1;
+
+ // data reconstruction
+ Acce_axis_data[0] = (sensorXM_data_read[1]<<8) | sensorXM_data_read[0]; // X
+ Acce_axis_data[1] = (sensorXM_data_read[3]<<8) | sensorXM_data_read[2]; // Y
+ Acce_axis_data[2] = (sensorXM_data_read[5]<<8) | sensorXM_data_read[4]; // Z
+}
+*/
+/*
+void init_can(void)
+{
+ CAN1SetOperationMode( CAN_IDLE_CON & CAN_MASTERCLOCK_1 &
+ CAN_REQ_OPERMODE_CONFIG );
+
+ while ( C1CTRLbits.OPMODE <= 3 );
+
+ CAN1Initialize( CAN_SYNC_JUMP_WIDTH4 & CAN_BAUD_PRE_SCALE(2),
+ CAN_WAKEUP_BY_FILTER_DIS &
+ CAN_PHASE_SEG2_TQ(3) &
+ CAN_PHASE_SEG1_TQ(3) &
+ CAN_PROPAGATIONTIME_SEG_TQ(3) &
+ CAN_SEG2_TIME_LIMIT_SET & CAN_SAMPLE3TIMES ) ;
+
+ CAN1SetFilter ( (char) 0 , CAN_FILTER_SID( 0x200 ) & CAN_RX_EID_DIS ,
+ CAN_FILTER_EID( 0x00 ) );
+
+
+ CAN1SetMask ( (char)0 , CAN_MASK_SID ( 0xFFF ) & CAN_MATCH_FILTER_TYPE ,
+ CAN_MASK_EID( 0x00) );
+
+
+ CAN1SetTXMode( (char) 0 , CAN_TX_STOP_REQ & CAN_TX_PRIORITY_HIGH );
+ CAN1SetRXMode( (char) 0 , CAN_RXFUL_CLEAR & CAN_BUF0_DBLBUFFER_DIS );
+ CAN1SetOperationMode( CAN_IDLE_CON & CAN_CAPTURE_DIS & CAN_MASTERCLOCK_1 &
+ CAN_REQ_OPERMODE_NOR );
+ C1INTEbits.RX0IE = 1;
+ C1INTEbits.RX1IE = 1;
+ IEC1bits.C1IE = 1;
+
+ C1TX0SID = 0x0000;
+ C1TX0EID = 0x0000;
+ C1RX0SID = 0x0000;
+ C1RXF0SID = 0x0010;
+}
+*/
+/*
+void _ISR _C1Interrupt(void)
+{
+ // 讀取大板子傳來的資料
+ if (C1RX0CONbits.RXFUL == 1)
+ {
+ rotationspeed = C1RX0B1;
+ omega = (float)rotationspeed * 0.4553 * 0.75; // 2pi * (10/138) (rad/s)
+ v = 0.33 * omega; // v = r*w (m/s)
+
+ C1RX0CONbits.RXFUL = 0;
+ }
+
+ C1INTFbits.RX0IF = 0;
+
+ IFS1bits.C1IF = 0;
+}
+*/
+/*
+void init_ADC(void)
+{
+ ADCON1bits.SSRC = 7; // Internal counter ends sampling and starts conversion (auto convert)
+ ADCON1bits.ASAM = 1; // Sampling begins immediately after last conversion completes
+ ADCON2bits.VCFG = 0; // A/D VREFH : AVDD(power) , A/D VREFL : AVSS(ground)
+ ADCON2bits.CHPS = 0; // 00 = Converts CH0
+ ADCON3bits.ADRC = 1; // A/D internal RC clock
+ ADCON3bits.SAMC = 16; // 16*TAD
+ ADCON3bits.ADCS = 11; // (TCY/2)*(ADCS<5:0>+1)=6*TCY , where ADCS<5:0>=11
+
+ ADCHS = 0x0006;; // Connect RB6/AN6 as CH0 input
+ ADPCFGbits.PCFG6 = 0; // 0 = Analog input pin in Analog mode ; 1: Digital mode
+
+ ADCON1bits.ADON = 1;
+ // 1 = A/D converter module is operating; 0 = A/D converter is off
+}
+*/
+
+float lpf(float input,float output_old,float frequency)
+{
+ float output = 0;
+
+ output = (output_old + frequency*T*input) / (1+frequency*T);
+
+ return output;
+}
+
+
+float sinr(float input)
+{
+ float output = 0;
+
+ output = input - input * input * input / 6;
+ return output;
+}
+
+
+float cosr(float input)
+{
+ float output = 0;
+
+ output = 1 - input * input / 2;
+ return output;
+}
+
+
+float arcsin(float input)
+{
+ float output = 0;
+
+ output = input + input * input * input / 6;
+ return output;
+}
+
+
+float arccos(float input)
+{
+ float output = 0;
+
+ output = (3.14 / 2) - arcsin(input);
+ return output;
+}
+
+
+float arctan(float input)
+{
+ float output = 0;
+
+ output = input - input * input * input / 3;
+ return output;
+}
+
+/*
+void _ISR _U2RXInterrupt(void)
+{
+ // 更改體重值
+ uart_read = U2RXREG;
+
+ switch(uart_read)
+ {
+ case 'q':
+ M = 45 + 25; // 人:45kg , 車:25kg
+ break;
+
+ case 'w':
+ M = 50 + 25;
+ break;
+
+ case 'e':
+ M = 55 + 25;
+ break;
+
+ case 'a':
+ M = 60 + 25;
+ break;
+
+ case 's':
+ M = 65 + 25;
+ break;
+
+ case 'd':
+ M = 70 + 25;
+ break;
+
+ case 'z':
+ M = 75 + 25;
+ break;
+
+ case 'x':
+ M = 80 + 25;
+ break;
+
+ case 'c':
+ M = 0;
+ break;
+ }
+
+ IFS1bits.U2RXIF = 0;
+}
+*/
void read_sensor(void)
{
// sensor raw data
@@ -250,413 +1084,8 @@
Acce_axis_data[0] = sensor.readRawAccelX();
Acce_axis_data[1] = sensor.readRawAccelY();
Acce_axis_data[2] = sensor.readRawAccelZ();
-}
-void init_Sensors(void)
-{
- //sensor_CSXM = 1;
- //sensor_CSG = 1;
- Gyro_axis_data[0] = 0; // X
- Gyro_axis_data[1] = 0; // Y
- Gyro_axis_data[2] = 0; // Z
- Acce_axis_data[0] = 0;
- Acce_axis_data[1] = 0;
- Acce_axis_data[2] = 0;
- //sensorG_setup();
- //sensorXM_setup();
-}
-float lpf(float input,float output_old,float frequency)
-{
- float output = 0;
- output = (output_old + frequency*T*input) / (1+frequency*T);
-
- return output;
-}
-
-
-float sinr(float input)
-{
- float output = 0;
-
- output = input - input * input * input / 6;
- return output;
-}
-
-
-float cosr(float input)
-{
- float output = 0;
-
- output = 1 - input * input / 2;
- return output;
-}
-
-
-float arcsin(float input)
-{
- float output = 0;
-
- output = input + input * input * input / 6;
- return output;
-}
-
-
-float arccos(float input)
-{
- float output = 0;
-
- output = (3.14 / 2) - arcsin(input);
- return output;
-}
-
-
-float arctan(float input)
-{
- float output = 0;
-
- output = input - input * input * input / 3;
- return output;
-}
-
-void separate(void)
-{
- num[2] = display[0] >> 8; // HSB(8bit)資料存入陣列
- num[3] = display[0] & 0x00ff; // LSB(8bit)資料存入陣列
- num[4] = display[1] >> 8;
- num[5] = display[1] & 0x00ff;
- num[6] = display[2] >> 8;
- num[7] = display[2] & 0x00ff;
- num[8] = display[3] >> 8;
- num[9] = display[3] & 0x00ff;
- num[10] = display[4] >> 8;
- num[11] = display[4] & 0x00ff;
- num[12] = display[5] >> 8;
- num[13] = display[5] & 0x00ff;
+ // Magn_axis_data[0] = sensor.readRawMagX();
+ // Magn_axis_data[1] = sensor.readRawMagY();
+ // Magn_axis_data[2] = sensor.readRawMagZ();
}
-
-void timer1_interrupt(void)
-{
- if((t.read_us() - lastMilli) >= LOOPTIME)
- { // 2000 us = 2 ms
- lastMilli = t.read_us();
-
- // sensor initial start
- if(init_flag == 0) //用timer代替 數超過300次 開始讀植
- {
- tcount = tcount + 1;
-
- if(tcount >= 300)
- {
- init_Sensors(); // 讓sensor完全上電後再初始化sensor
- tcount = 0;
- init_flag = 1;
- }
- }
- else
- {
- read_sensor();
-
- // gyro filter
- Gyro_axis_data_f[0] = lpf(Gyro_axis_data[0],Gyro_axis_data_f_old[0],18);
- Gyro_axis_data_f_old[0] = Gyro_axis_data_f[0];
- Gyro_axis_data_f[1] = lpf(Gyro_axis_data[1],Gyro_axis_data_f_old[1],18);
- Gyro_axis_data_f_old[1] = Gyro_axis_data_f[1];
- Gyro_axis_data_f[2] = lpf(Gyro_axis_data[2],Gyro_axis_data_f_old[2],18);
- Gyro_axis_data_f_old[2] = Gyro_axis_data_f[2];
-
-
- // gyro bias average
- if(gyro_flag == 0)
- {
- gyro_count = gyro_count + 1;
-
- if(gyro_count == 500) {
- gyro_init_1[index_count] = Gyro_axis_data_f[0];
- gyro_init_2[index_count] = Gyro_axis_data_f[1];
- gyro_init_3[index_count] = Gyro_axis_data_f[2];
- index_count++;
- } else if(gyro_count == 505) {
- gyro_init_1[index_count] = Gyro_axis_data_f[0];
- gyro_init_2[index_count] = Gyro_axis_data_f[1];
- gyro_init_3[index_count] = Gyro_axis_data_f[2];
- index_count++;
- } else if(gyro_count == 510) {
- gyro_init_1[index_count] = Gyro_axis_data_f[0];
- gyro_init_2[index_count] = Gyro_axis_data_f[1];
- gyro_init_3[index_count] = Gyro_axis_data_f[2];
- index_count++;
- } else if(gyro_count == 520) {
- gyro_init_1[index_count] = Gyro_axis_data_f[0];
- gyro_init_2[index_count] = Gyro_axis_data_f[1];
- gyro_init_3[index_count] = Gyro_axis_data_f[2];
- index_count++;
- } else if(gyro_count == 530) {
- gyro_init_1[index_count] = Gyro_axis_data_f[0];
- gyro_init_2[index_count] = Gyro_axis_data_f[1];
- gyro_init_3[index_count] = Gyro_axis_data_f[2];
- index_count++;
- } else if(gyro_count == 550) {
- gyro_init_1[index_count] = Gyro_axis_data_f[0];
- gyro_init_2[index_count] = Gyro_axis_data_f[1];
- gyro_init_3[index_count] = Gyro_axis_data_f[2];
- index_count++;
- } else if(gyro_count == 560) {
- gyro_init_1[index_count] = Gyro_axis_data_f[0];
- gyro_init_2[index_count] = Gyro_axis_data_f[1];
- gyro_init_3[index_count] = Gyro_axis_data_f[2];
- index_count++;
- } else if(gyro_count == 570) {
- gyro_init_1[index_count] = Gyro_axis_data_f[0];
- gyro_init_2[index_count] = Gyro_axis_data_f[1];
- gyro_init_3[index_count] = Gyro_axis_data_f[2];
- index_count++;
- } else if(gyro_count == 580) {
- gyro_init_1[index_count] = Gyro_axis_data_f[0];
- gyro_init_2[index_count] = Gyro_axis_data_f[1];
- gyro_init_3[index_count] = Gyro_axis_data_f[2];
- index_count++;
- } else if(gyro_count >= 590) {
- gyro_init_1[index_count] = Gyro_axis_data_f[0];
- gyro_init_2[index_count] = Gyro_axis_data_f[1];
- gyro_init_3[index_count] = Gyro_axis_data_f[2];
- int z = 0;
- for(z=0 ; z<10 ; z++) {
- gyro_sum_1 = gyro_sum_1 + gyro_init_1[z];
- gyro_sum_2 = gyro_sum_2 + gyro_init_2[z];
- gyro_sum_3 = gyro_sum_3 + gyro_init_3[z];
- }
-
- Gyro_axis_zero[0] = gyro_sum_1 / 10;
- Gyro_axis_zero[1] = gyro_sum_2 / 10;
- Gyro_axis_zero[2] = gyro_sum_3 / 10;
- index_count = 0;
- gyro_count = 0;
- gyro_flag = 1;
- }
- }
-
- //831 2:20
-
- // acce filter
- Acce_axis_data_f[0] = lpf(Acce_axis_data[0],Acce_axis_data_f_old[0],18);
- Acce_axis_data_f_old[0] = Acce_axis_data_f[0];
- Acce_axis_data_f[1] = lpf(Acce_axis_data[1],Acce_axis_data_f_old[1],18);
- Acce_axis_data_f_old[1] = Acce_axis_data_f[1];
- Acce_axis_data_f[2] = lpf(Acce_axis_data[2],Acce_axis_data_f_old[2],18);
- Acce_axis_data_f_old[2] = Acce_axis_data_f[2];
-
- Gyro_scale[0] = (Gyro_axis_data_f[0]-Gyro_axis_zero[0])*Gyro_gain_x;
- Gyro_scale[1] = (Gyro_axis_data_f[1]-Gyro_axis_zero[1])*Gyro_gain_y;
- Gyro_scale[2] = (Gyro_axis_data_f[2]-Gyro_axis_zero[2])*Gyro_gain_z;
-
- Acce_scale[0] = ((Acce_axis_data_f[0]-Acce_axis_zero[0]))*Acce_gainx;
- Acce_scale[1] = ((Acce_axis_data_f[1]-Acce_axis_zero[1]))*Acce_gainy;
- Acce_scale[2] = ((Acce_axis_data_f[2]-Acce_axis_zero[2]))*Acce_gainz;
-
- // final 6 axis data
- axm = Acce_scale[0];
- aym = Acce_scale[1];
- azm = Acce_scale[2];
-
- u1 = Gyro_scale[0];
- u2 = Gyro_scale[1];
- u3 = Gyro_scale[2];
-
- // new defined direction
- ax = -aym;
- ay = axm;
- az = azm;
-
- w1 = -u2;
- w2 = u1;
- w3 = u3;
-
- // w deadzone
- int flag_d_1 = 0;
- int flag_d_2 = 0;
-
- flag_d_1 = w1 <= 0.015;
- flag_d_2 = w1 >= -0.015;
-
- if((flag_d_1 + flag_d_2) == 2)
- w1_d = 0;
- else
- w1_d = w1;
-
- int flag_d_3 = 0;
- int flag_d_4 = 0;
- flag_d_3 = w2 <= 0.015;
- flag_d_4 = w2 >= -0.015;
- if((flag_d_3 + flag_d_4) == 2)
- w2_d = 0;
- else
- w2_d = w2;
-
- int flag_d_5 = 0;
- int flag_d_6 = 0;
- flag_d_5 = w3 <= 0.015;
- flag_d_6 = w3 >= -0.015;
- if((flag_d_5 + flag_d_6) == 2)
- w3_d = 0;
- else
- w3_d = w3;
- }
-
- // velocity filter
- v_f = lpf(v,v_f_old,18);
- v_f_old = v_f;
-
- // 離心加速度
- acy_hat = 1.4*v_f*w1_d; // 1.2
- acy_hat_f = lpf(acy_hat,acy_hat_f_old,18);
- acy_hat_f_old = acy_hat_f;
-
- // sensor fusion by backward Euler method
- gs1_hat = (gs1_hat_old + T*(-w2_d*alpf_hat + p*ax - w3_d*(acy_hat_f-ay) - w2_d*az)) / (1+p*T);
- gs1_hat_old = gs1_hat;
-
- gs2_hat = (gs2_hat_old + T*(w1_d*alpf_hat - w3_d*ax - p*(acy_hat_f-ay) + w1_d*az)) / (1+p*T);
- gs2_hat_old = gs2_hat;
-
- gs3_hat = (gs3_hat_old + T*(-b*alpf_hat + c*v_hat + w2_d*ax + w1_d*(acy_hat_f-ay) - b*az - c*v_f)) / (1-b*T);
- gs3_hat_old = gs3_hat;
-
- alpf_hat = (alpf_hat_old + T*((a+d)*gs3_hat + e*v_hat - (a+d)*az - e*v_f)) / (1+(a+d)*T);
- alpf_hat_old = alpf_hat;
-
- v_hat = (v_hat_old + T*((1+f)*gs3_hat - f*alpf_hat - (1+f)*az - g*v_f)) / (1-g*T);
- v_hat_old = v_hat;
-
- /*
- // 離心加速度
- acy_hat = 1.2*v_f*w1;
- acy_hat_f = lpf(acy_hat,acy_hat_f_old,18);
- acy_hat_f_old = acy_hat_f;
-
- // sensor fusion by backward Euler method
- gs1_hat = (gs1_hat_old + T*(-w2*alpf_hat + p*ax - w3*(acy_hat_f-ay) - w2*az)) / (1+p*T);
- gs1_hat_old = gs1_hat;
-
- gs2_hat = (gs2_hat_old + T*(w1*alpf_hat - w3*ax - p*(acy_hat_f-ay) + w1*az)) / (1+p*T);
- gs2_hat_old = gs2_hat;
-
- gs3_hat = (gs3_hat_old + T*(-b*alpf_hat + c*v_hat + w2*ax + w1*(acy_hat_f-ay) - b*az - c*v_f)) / (1-b*T);
- gs3_hat_old = gs3_hat;
-
- alpf_hat = (alpf_hat_old + T*((a+d)*gs3_hat + e*v_hat - (a+d)*az - e*v_f)) / (1+(a+d)*T);
- alpf_hat_old = alpf_hat;
-
- v_hat = (v_hat_old + T*((1+f)*gs3_hat - f*alpf_hat - (1+f)*az - g*v_f)) / (1-g*T);
- v_hat_old = v_hat;
- */
-
- // normalize gs1_hat , gs2_hat , gs3_hat to find pitch angle and roll angle
- n = sqrt(gs1_hat*gs1_hat + gs2_hat*gs2_hat + gs3_hat*gs3_hat);
-
- gs1_hat_n = (gs1_hat / n) * 9.81;
- gs2_hat_n = (gs2_hat / n) * 9.81;
- gs3_hat_n = (gs3_hat / n) * 9.81;
-
- psi = arcsin(-gs3_hat_n / 9.81);
- //theta = atan(-gs2_hat_n / gs1_hat_n);
-
- // psi correction
- psi_m = psi - (0); // after correction
- psi_deg = 57.3 * psi_m;
-
- // psi deadzone
- int flag_d_1 = 0;
- int flag_d_2 = 0;
- flag_d_1 = psi_deg < 2.5; // 在平地時 flag_d_1 = 1
- flag_d_2 = psi_deg > -3; // 在平地時 flag_d_2 = 1
-
- if((flag_d_1 + flag_d_2) == 2) // 在平地時 flag_d_1 + flag_d_2 = 2
- psi_d = 0;
- else
- psi_d = psi_m;
-
-
- // 去除負加速度
- if(alpf_hat < 0)
- alpf_c = 0;
- else
- alpf_c = alpf_hat;
-
- // 速度為負時不考慮加速度
- if(v_f < 0)
- alpf_v = 0;
- else
- alpf_v = alpf_c;
-
- // 加速度 deadzone
- if(alpf_v <= 0.1)
- alpf_d = 0;
- else
- alpf_d = alpf_v;
-
-
- // buttom filter // 改button
- /*
- buttom_buf = lpf(ADCBUF0,buttom_buf_old,30);
- buttom_buf_old = buttom_buf;
-
- // button control compensationrate
- if(buttom_buf >= 450)
- compensationrate = 1;
- else
- compensationrate = 0;
- */
-
- // 電流命令 (T = k*i)
- current_command = compensationrate * (Mb*alpf_d*1.1 + M*9.81*sinr(psi_d)*1.35 + Df) * R/Ki;
-
- // 電流命令上下限
- if(current_command >= 15)
- current_command = 15;
- else if(current_command <= -25)
- current_command = -25;
- //831 412
-
-
- // 開關安全措施
- b_count = b_count + 1;
-
- if(b_count >= 1500) // wait for buttom ADC buffer ready
- {
- b_init_flag = 1;
- b_count = 0;
- }
-
- if(b_init_flag == 0)
- open_flag = 1;
- else if(b_init_flag == 1)
- {
- if(compensationrate == 1 && open_flag == 1) // 初始 open_flag = 1
- protect_flag = 1;
-
- if(compensationrate == 0) // 初始 open_flag = 1 , 等 compensationrate = 0 時才將 open_flag = 0
- {
- protect_flag = 0; // 初始 compensationrate = 0 時才將 protect_flag = 0
- open_flag = 0;
- b_flag = 0;
- }
- // 要先有初始 compensationrate = 0 才能先將 protect_flag == 0 && open_flag == 0 成立
- // 之後由 compensationrate = 0 轉變成 compensationrate = 1 時才將 b_flag = 1(開始送命令)
- if(compensationrate == 1 && protect_flag == 0 && open_flag == 0)
- {
- b_flag = 1;
- }
- }
-
- // 改CAN
-
-
- i = 0; //uart_send() neen initial i (i=0)
- uart_send();
- }
-}
-
-void init_TIMER(void)
-{
- timer1.attach_us(&timer1_interrupt, 2000); //20ms
-}
\ No newline at end of file