LDSC_Robotics_TAs / Mbed 2 deprecated Bike_Sensor_Fusion

Version_1

Dependencies:   LSM9DS0 mbed

main.cpp@1:235bfa26c5c1, 2016-09-05 (annotated)

Committer:
noname001
Date:
Mon Sep 05 02:02:19 2016 +0000
Revision:
1:235bfa26c5c1
Parent:
0:0b8d14ed499f 
Version_2 (Date:2016-09-05)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
noname001 1:235bfa26c5c1 1 // 導入需要之header檔
noname001 1:235bfa26c5c1 2 //#include <p30f4011.h>
noname001 0:0b8d14ed499f 3 #include "mbed.h"
noname001 0:0b8d14ed499f 4 #include <math.h>
noname001 0:0b8d14ed499f 5 #include "LSM9DS0.h"
noname001 0:0b8d14ed499f 6
noname001 0:0b8d14ed499f 7
noname001 1:235bfa26c5c1 8 Ticker timer1; // 2 ms
noname001 1:235bfa26c5c1 9 Serial uart_1(D10,D2); // TX : D10 RX : D2 // serial 1
noname001 1:235bfa26c5c1 10 LSM9DS0 sensor(SPI_MODE, D7, D6); // SPI_CS1 : D7 , SPI_CS2 : D6
noname001 1:235bfa26c5c1 11 AnalogIn adc_switch(A0);
noname001 0:0b8d14ed499f 12 // uart data
noname001 0:0b8d14ed499f 13 int display[6] = {0,0,0,0,0,0};
noname001 1:235bfa26c5c1 14 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
noname001 0:0b8d14ed499f 15
noname001 1:235bfa26c5c1 16 // 等待sensor上電
noname001 0:0b8d14ed499f 17 int tcount = 0;
noname001 0:0b8d14ed499f 18 int init_flag = 0;
noname001 0:0b8d14ed499f 19
noname001 1:235bfa26c5c1 20 int flag_d_1;
noname001 1:235bfa26c5c1 21 int flag_d_2;
noname001 1:235bfa26c5c1 22 int flag_d_3;
noname001 1:235bfa26c5c1 23 int flag_d_4;
noname001 1:235bfa26c5c1 24 int flag_d_5;
noname001 1:235bfa26c5c1 25 int flag_d_6;
noname001 0:0b8d14ed499f 26 int i;
noname001 0:0b8d14ed499f 27 int j;
noname001 0:0b8d14ed499f 28 int k;
noname001 1:235bfa26c5c1 29 int z;
noname001 1:235bfa26c5c1 30 float T = 0.002;
noname001 0:0b8d14ed499f 31 // sensor gain , 由實驗測得
noname001 1:235bfa26c5c1 32 //#define Gyro_gain_x 0.001223735681818
noname001 1:235bfa26c5c1 33 #define Gyro_gain_x 0.001223735681818f
noname001 1:235bfa26c5c1 34 #define Gyro_gain_y 0.001223735681818f
noname001 1:235bfa26c5c1 35 #define Gyro_gain_z 0.001223735681818f
noname001 1:235bfa26c5c1 36 #define Acce_gainx -0.002386188420346f // -9.81 / ( 3264.287 - (-846.872) )
noname001 0:0b8d14ed499f 37 //#define Acce_gainy -0.002372964173321 // -9.81 / ( 3508 - (-626.07) )
noname001 1:235bfa26c5c1 38 #define Acce_gainy -0.002512964173321f
noname001 1:235bfa26c5c1 39 #define Acce_gainz -0.002381154075718f // -9.81 / ( 4413.257 - (293.406) )
noname001 0:0b8d14ed499f 40
noname001 0:0b8d14ed499f 41 // 宣告從sensor讀到的值要存入處理的變數
noname001 1:235bfa26c5c1 42 int16_t Gyro_axis_data[3]; // X, Y, Z axis
noname001 0:0b8d14ed499f 43 float Gyro_axis_data_f[3];
noname001 0:0b8d14ed499f 44 float Gyro_axis_data_f_old[3];
noname001 0:0b8d14ed499f 45 float Gyro_scale[3]; // scale = (data - zero) * gain
noname001 1:235bfa26c5c1 46 float Gyro_axis_zero[3] = {0,-8,8};
noname001 0:0b8d14ed499f 47
noname001 1:235bfa26c5c1 48 int16_t Acce_axis_data[3]; // X, Y, Z axis
noname001 0:0b8d14ed499f 49 float Acce_axis_data_f[3];
noname001 0:0b8d14ed499f 50 float Acce_axis_data_f_old[3];
noname001 0:0b8d14ed499f 51 float Acce_scale[3]; // scale = (data - zero) * gain
noname001 0:0b8d14ed499f 52 //float Acce_axis_zero[3] = {-846.872,-626.07,293.406};
noname001 1:235bfa26c5c1 53 float Acce_axis_zero[3] = {-907,-658,225};
noname001 0:0b8d14ed499f 54
noname001 0:0b8d14ed499f 55 // final sensor output value
noname001 0:0b8d14ed499f 56 float axm = 0; // Acce_scale
noname001 0:0b8d14ed499f 57 float aym = 0; // Acce_scale
noname001 0:0b8d14ed499f 58 float azm = 0; // Acce_scale
noname001 0:0b8d14ed499f 59
noname001 0:0b8d14ed499f 60 float u1 = 0; // Gyro_scale
noname001 0:0b8d14ed499f 61 float u2 = 0; // Gyro_scale
noname001 0:0b8d14ed499f 62 float u3 = 0; // Gyro_scale
noname001 0:0b8d14ed499f 63
noname001 1:235bfa26c5c1 64 // counter for gyro bias //improve
noname001 0:0b8d14ed499f 65 int gyro_count = 0;
noname001 0:0b8d14ed499f 66 int gyro_flag = 0;
noname001 0:0b8d14ed499f 67 float gyro_init_1[10] = {0,0,0,0,0,0,0,0,0,0};
noname001 0:0b8d14ed499f 68 float gyro_init_2[10] = {0,0,0,0,0,0,0,0,0,0};
noname001 0:0b8d14ed499f 69 float gyro_init_3[10] = {0,0,0,0,0,0,0,0,0,0};
noname001 0:0b8d14ed499f 70 int index_count = 0;
noname001 0:0b8d14ed499f 71 float gyro_sum_1 = 0;
noname001 0:0b8d14ed499f 72 float gyro_sum_2 = 0;
noname001 0:0b8d14ed499f 73 float gyro_sum_3 = 0;
noname001 0:0b8d14ed499f 74
noname001 0:0b8d14ed499f 75 // new defined direction
noname001 1:235bfa26c5c1 76 float ax = 0;
noname001 1:235bfa26c5c1 77 float ay = 0;
noname001 0:0b8d14ed499f 78 float az = 0;
noname001 0:0b8d14ed499f 79
noname001 1:235bfa26c5c1 80 float w1 = 0;
noname001 1:235bfa26c5c1 81 float w2 = 0;
noname001 0:0b8d14ed499f 82 float w3 = 0;
noname001 0:0b8d14ed499f 83
noname001 1:235bfa26c5c1 84 float w1_d = 0;
noname001 1:235bfa26c5c1 85 float w2_d = 0;
noname001 0:0b8d14ed499f 86 float w3_d = 0;
noname001 0:0b8d14ed499f 87
noname001 1:235bfa26c5c1 88 //read weight
noname001 1:235bfa26c5c1 89 //char uart_read = 0;
noname001 0:0b8d14ed499f 90
noname001 0:0b8d14ed499f 91 // 速度 , 向心加速度
noname001 0:0b8d14ed499f 92 int rotationspeed = 0;
noname001 0:0b8d14ed499f 93 float omega = 0;
noname001 0:0b8d14ed499f 94 float v = 0;
noname001 0:0b8d14ed499f 95 float v_f = 0;
noname001 0:0b8d14ed499f 96 float v_f_old = 0;
noname001 0:0b8d14ed499f 97 float acy_hat = 0;
noname001 0:0b8d14ed499f 98 float acy_hat_f = 0;
noname001 0:0b8d14ed499f 99 float acy_hat_f_old = 0;
noname001 0:0b8d14ed499f 100
noname001 0:0b8d14ed499f 101 // estimated state variables
noname001 0:0b8d14ed499f 102 float gs1_hat = 0;
noname001 0:0b8d14ed499f 103 float gs1_hat_old = 0;
noname001 0:0b8d14ed499f 104
noname001 0:0b8d14ed499f 105 float gs2_hat = 0;
noname001 0:0b8d14ed499f 106 float gs2_hat_old = 0;
noname001 0:0b8d14ed499f 107
noname001 0:0b8d14ed499f 108 float gs3_hat = 0;
noname001 0:0b8d14ed499f 109 float gs3_hat_old = 0;
noname001 0:0b8d14ed499f 110
noname001 0:0b8d14ed499f 111 float alpf_hat = 0;
noname001 0:0b8d14ed499f 112 float alpf_hat_old = 0;
noname001 0:0b8d14ed499f 113
noname001 0:0b8d14ed499f 114 float v_hat = 0;
noname001 0:0b8d14ed499f 115 float v_hat_old = 0;
noname001 0:0b8d14ed499f 116
noname001 0:0b8d14ed499f 117 // La , Lb
noname001 0:0b8d14ed499f 118 #define pi 3.14
noname001 0:0b8d14ed499f 119 float a = 1.1304; // alpha
noname001 0:0b8d14ed499f 120 float p = 1.1304; // La (0.18 Hz)
noname001 0:0b8d14ed499f 121
noname001 0:0b8d14ed499f 122 // butterworth filter
noname001 0:0b8d14ed499f 123 float b = -0.5652; // Lb (0.18 Hz)
noname001 0:0b8d14ed499f 124 float c = -1.2778;
noname001 0:0b8d14ed499f 125 float d = -0.5652;
noname001 0:0b8d14ed499f 126 float e = -1.2778;
noname001 0:0b8d14ed499f 127 float f = -0.5000;
noname001 0:0b8d14ed499f 128 float g = -1.1304;
noname001 0:0b8d14ed499f 129
noname001 0:0b8d14ed499f 130 // angle
noname001 0:0b8d14ed499f 131 float psi = 0; // pitch angle
noname001 0:0b8d14ed499f 132 float psi_deg = 0;
noname001 0:0b8d14ed499f 133 float theta = 0; // roll angle
noname001 0:0b8d14ed499f 134 float theta_deg = 0;
noname001 0:0b8d14ed499f 135
noname001 0:0b8d14ed499f 136 // normalized variables
noname001 1:235bfa26c5c1 137 float n = 0;
noname001 0:0b8d14ed499f 138 float gs1_hat_n = 0;
noname001 0:0b8d14ed499f 139 float gs2_hat_n = 0;
noname001 0:0b8d14ed499f 140 float gs3_hat_n = 0;
noname001 0:0b8d14ed499f 141
noname001 0:0b8d14ed499f 142 int can_receive_1 = 0;
noname001 0:0b8d14ed499f 143 int can_receive_2 = 0;
noname001 0:0b8d14ed499f 144 int can_receive_3 = 0;
noname001 0:0b8d14ed499f 145
noname001 0:0b8d14ed499f 146 // correction
noname001 0:0b8d14ed499f 147 float psi_c = 0;
noname001 0:0b8d14ed499f 148 float psi_m = 0;
noname001 0:0b8d14ed499f 149 float psi_d = 0;
noname001 0:0b8d14ed499f 150 float alpf_v = 0;
noname001 0:0b8d14ed499f 151 float alpf_c = 0;
noname001 0:0b8d14ed499f 152 float alpf_d = 0;
noname001 0:0b8d14ed499f 153
noname001 0:0b8d14ed499f 154 int current_command = 0;
noname001 0:0b8d14ed499f 155 float Ki = -1.211; // motor constant
noname001 0:0b8d14ed499f 156 float R = 0.33; // wheel radius
noname001 0:0b8d14ed499f 157 float Df = 12; // damping force
noname001 0:0b8d14ed499f 158 int compensationrate = 0;
noname001 0:0b8d14ed499f 159 int Mb = 25; // bicycle mass
noname001 0:0b8d14ed499f 160 int M = 95; // total mass (25+70)
noname001 1:235bfa26c5c1 161 int adc = 0;
noname001 0:0b8d14ed499f 162 int buttom_buf = 0;
noname001 0:0b8d14ed499f 163 int buttom_buf_old = 0;
noname001 0:0b8d14ed499f 164 // 開關安全措施
noname001 0:0b8d14ed499f 165 int b_count = 0;
noname001 0:0b8d14ed499f 166 int b_init_flag = 0;
noname001 0:0b8d14ed499f 167 int b_flag = 0;
noname001 0:0b8d14ed499f 168 int open_flag = 0;
noname001 0:0b8d14ed499f 169 int protect_flag = 0;
noname001 0:0b8d14ed499f 170
noname001 1:235bfa26c5c1 171
noname001 1:235bfa26c5c1 172 // 函式宣告
noname001 0:0b8d14ed499f 173 void init_uart(void);
noname001 0:0b8d14ed499f 174 void separate(void);
noname001 0:0b8d14ed499f 175 void init_Sensors(void);
noname001 1:235bfa26c5c1 176 void init_TIMER();
noname001 1:235bfa26c5c1 177 void timer1_interrupt(void); // void _ISR _T1Interrupt(void);
noname001 0:0b8d14ed499f 178 void read_sensor(void);
noname001 1:235bfa26c5c1 179
noname001 1:235bfa26c5c1 180 //unsigned char SPI_operation(unsigned char data_send);
noname001 1:235bfa26c5c1 181 //void sensorG_setup(void);
noname001 1:235bfa26c5c1 182 //void sensorXM_setup(void);
noname001 1:235bfa26c5c1 183 //void sensorG_read_3axis(void);
noname001 1:235bfa26c5c1 184 //void sensorXM_read_3axis(void);
noname001 1:235bfa26c5c1 185 //void init_ADC(void);
noname001 1:235bfa26c5c1 186 //void _ISR _U2RXInterrupt(void);
noname001 1:235bfa26c5c1 187 //void init_IO(void);
noname001 1:235bfa26c5c1 188 // void init_spi(void);
noname001 1:235bfa26c5c1 189
noname001 0:0b8d14ed499f 190 float lpf(float input,float output_old,float frequency);
noname001 0:0b8d14ed499f 191 float sinr(float input);
noname001 0:0b8d14ed499f 192 float cosr(float input);
noname001 0:0b8d14ed499f 193 float arcsin(float input);
noname001 0:0b8d14ed499f 194 float arccos(float input);
noname001 0:0b8d14ed499f 195 float arctan(float input);
noname001 0:0b8d14ed499f 196
noname001 0:0b8d14ed499f 197
noname001 0:0b8d14ed499f 198
noname001 1:235bfa26c5c1 199 // Main routine
noname001 1:235bfa26c5c1 200 int main(void)
noname001 1:235bfa26c5c1 201 {
noname001 0:0b8d14ed499f 202 //init_IO();
noname001 1:235bfa26c5c1 203 //init_spi();
noname001 1:235bfa26c5c1 204 //init_ADC();
noname001 1:235bfa26c5c1 205 init_TIMER();
noname001 0:0b8d14ed499f 206 init_uart();
noname001 1:235bfa26c5c1 207
noname001 1:235bfa26c5c1 208 while(1) // 永久迴圈
noname001 1:235bfa26c5c1 209 {
noname001 1:235bfa26c5c1 210
noname001 1:235bfa26c5c1 211 }
noname001 1:235bfa26c5c1 212
noname001 1:235bfa26c5c1 213 } // End of main()
noname001 0:0b8d14ed499f 214
noname001 1:235bfa26c5c1 215 /*
noname001 1:235bfa26c5c1 216 void init_IO(void)
noname001 1:235bfa26c5c1 217 {
noname001 1:235bfa26c5c1 218 // 1為數位，0為類比
noname001 1:235bfa26c5c1 219 ADPCFG = 0xffff;
noname001 1:235bfa26c5c1 220
noname001 1:235bfa26c5c1 221 //LED
noname001 1:235bfa26c5c1 222 TRISDbits.TRISD1 = 0; // 定義I/O腳位輸入輸出 0:output 1:input
noname001 1:235bfa26c5c1 223 TRISDbits.TRISD2 = 0;
noname001 1:235bfa26c5c1 224 TRISDbits.TRISD3 = 0;
noname001 1:235bfa26c5c1 225 LATDbits.LATD1 = 1; // 1:dark 0:light
noname001 1:235bfa26c5c1 226 LATDbits.LATD2 = 1;
noname001 1:235bfa26c5c1 227 LATDbits.LATD3 = 1;
noname001 1:235bfa26c5c1 228
noname001 1:235bfa26c5c1 229 //SPI CS
noname001 1:235bfa26c5c1 230 TRISBbits.TRISB4 = 0; // CSXM
noname001 1:235bfa26c5c1 231 TRISBbits.TRISB5 = 0; // CSG
noname001 1:235bfa26c5c1 232 LATBbits.LATB4 = 1;
noname001 1:235bfa26c5c1 233 LATBbits.LATB5 = 1;
noname001 1:235bfa26c5c1 234
noname001 1:235bfa26c5c1 235 //ADC
noname001 1:235bfa26c5c1 236 ADPCFGbits.PCFG6 = 1; // AN6 = digital
noname001 1:235bfa26c5c1 237 TRISBbits.TRISB6 = 1; // input 讀取電壓
noname001 1:235bfa26c5c1 238 }
noname001 1:235bfa26c5c1 239 */
noname001 0:0b8d14ed499f 240
noname001 1:235bfa26c5c1 241 void init_TIMER(){
noname001 1:235bfa26c5c1 242 i = 0;
noname001 1:235bfa26c5c1 243 timer1.attach_us(&timer1_interrupt, 2000.0); // 2 ms
noname001 0:0b8d14ed499f 244 }
noname001 0:0b8d14ed499f 245
noname001 0:0b8d14ed499f 246
noname001 1:235bfa26c5c1 247 //void _ISR1 _T1Interrupt(void) // 500Hz
noname001 1:235bfa26c5c1 248 void timer1_interrupt(void)
noname001 0:0b8d14ed499f 249 {
noname001 1:235bfa26c5c1 250 //LATDbits.LATD3 = 1;
noname001 1:235bfa26c5c1 251
noname001 1:235bfa26c5c1 252 if(init_flag == 0) //用timer代替 數超過300次 開始讀植
noname001 1:235bfa26c5c1 253 {
noname001 1:235bfa26c5c1 254 tcount = tcount + 1;
noname001 1:235bfa26c5c1 255
noname001 1:235bfa26c5c1 256 if(tcount >= 300)
noname001 1:235bfa26c5c1 257 {
noname001 1:235bfa26c5c1 258 init_Sensors(); // 讓sensor完全上電後再初始化sensor
noname001 1:235bfa26c5c1 259 tcount = 0;
noname001 1:235bfa26c5c1 260 init_flag = 1;
noname001 1:235bfa26c5c1 261 }
noname001 1:235bfa26c5c1 262 }
noname001 1:235bfa26c5c1 263
noname001 1:235bfa26c5c1 264 else
noname001 1:235bfa26c5c1 265 {
noname001 1:235bfa26c5c1 266 read_sensor();
noname001 1:235bfa26c5c1 267
noname001 1:235bfa26c5c1 268 // gyro filter
noname001 1:235bfa26c5c1 269 Gyro_axis_data_f[0] = lpf(Gyro_axis_data[0],Gyro_axis_data_f_old[0],18);
noname001 1:235bfa26c5c1 270 Gyro_axis_data_f_old[0] = Gyro_axis_data_f[0];
noname001 1:235bfa26c5c1 271 Gyro_axis_data_f[1] = lpf(Gyro_axis_data[1],Gyro_axis_data_f_old[1],18);
noname001 1:235bfa26c5c1 272 Gyro_axis_data_f_old[1] = Gyro_axis_data_f[1];
noname001 1:235bfa26c5c1 273 Gyro_axis_data_f[2] = lpf(Gyro_axis_data[2],Gyro_axis_data_f_old[2],18);
noname001 1:235bfa26c5c1 274 Gyro_axis_data_f_old[2] = Gyro_axis_data_f[2];
noname001 1:235bfa26c5c1 275
noname001 1:235bfa26c5c1 276 // gyro bias average
noname001 1:235bfa26c5c1 277 if(gyro_flag == 0)
noname001 1:235bfa26c5c1 278 {
noname001 1:235bfa26c5c1 279 gyro_count = gyro_count + 1;
noname001 1:235bfa26c5c1 280
noname001 1:235bfa26c5c1 281 if(gyro_count == 500)
noname001 1:235bfa26c5c1 282 {
noname001 1:235bfa26c5c1 283 gyro_init_1[index_count] = Gyro_axis_data_f[0];
noname001 1:235bfa26c5c1 284 gyro_init_2[index_count] = Gyro_axis_data_f[1];
noname001 1:235bfa26c5c1 285 gyro_init_3[index_count] = Gyro_axis_data_f[2];
noname001 1:235bfa26c5c1 286 index_count++;
noname001 1:235bfa26c5c1 287 }
noname001 1:235bfa26c5c1 288 else if(gyro_count == 505)
noname001 1:235bfa26c5c1 289 {
noname001 1:235bfa26c5c1 290 gyro_init_1[index_count] = Gyro_axis_data_f[0];
noname001 1:235bfa26c5c1 291 gyro_init_2[index_count] = Gyro_axis_data_f[1];
noname001 1:235bfa26c5c1 292 gyro_init_3[index_count] = Gyro_axis_data_f[2];
noname001 1:235bfa26c5c1 293 index_count++;
noname001 1:235bfa26c5c1 294 }
noname001 1:235bfa26c5c1 295 else if(gyro_count == 510)
noname001 1:235bfa26c5c1 296 {
noname001 1:235bfa26c5c1 297 gyro_init_1[index_count] = Gyro_axis_data_f[0];
noname001 1:235bfa26c5c1 298 gyro_init_2[index_count] = Gyro_axis_data_f[1];
noname001 1:235bfa26c5c1 299 gyro_init_3[index_count] = Gyro_axis_data_f[2];
noname001 1:235bfa26c5c1 300 index_count++;
noname001 1:235bfa26c5c1 301 }
noname001 1:235bfa26c5c1 302 else if(gyro_count == 520)
noname001 1:235bfa26c5c1 303 {
noname001 1:235bfa26c5c1 304 gyro_init_1[index_count] = Gyro_axis_data_f[0];
noname001 1:235bfa26c5c1 305 gyro_init_2[index_count] = Gyro_axis_data_f[1];
noname001 1:235bfa26c5c1 306 gyro_init_3[index_count] = Gyro_axis_data_f[2];
noname001 1:235bfa26c5c1 307 index_count++;
noname001 1:235bfa26c5c1 308 }
noname001 1:235bfa26c5c1 309 else if(gyro_count == 530)
noname001 1:235bfa26c5c1 310 {
noname001 1:235bfa26c5c1 311 gyro_init_1[index_count] = Gyro_axis_data_f[0];
noname001 1:235bfa26c5c1 312 gyro_init_2[index_count] = Gyro_axis_data_f[1];
noname001 1:235bfa26c5c1 313 gyro_init_3[index_count] = Gyro_axis_data_f[2];
noname001 1:235bfa26c5c1 314 index_count++;
noname001 1:235bfa26c5c1 315 }
noname001 1:235bfa26c5c1 316 else if(gyro_count == 550)
noname001 1:235bfa26c5c1 317 {
noname001 1:235bfa26c5c1 318 gyro_init_1[index_count] = Gyro_axis_data_f[0];
noname001 1:235bfa26c5c1 319 gyro_init_2[index_count] = Gyro_axis_data_f[1];
noname001 1:235bfa26c5c1 320 gyro_init_3[index_count] = Gyro_axis_data_f[2];
noname001 1:235bfa26c5c1 321 index_count++;
noname001 1:235bfa26c5c1 322 }
noname001 1:235bfa26c5c1 323 else if(gyro_count == 560)
noname001 1:235bfa26c5c1 324 {
noname001 1:235bfa26c5c1 325 gyro_init_1[index_count] = Gyro_axis_data_f[0];
noname001 1:235bfa26c5c1 326 gyro_init_2[index_count] = Gyro_axis_data_f[1];
noname001 1:235bfa26c5c1 327 gyro_init_3[index_count] = Gyro_axis_data_f[2];
noname001 1:235bfa26c5c1 328 index_count++;
noname001 1:235bfa26c5c1 329 }
noname001 1:235bfa26c5c1 330 else if(gyro_count == 570)
noname001 1:235bfa26c5c1 331 {
noname001 1:235bfa26c5c1 332 gyro_init_1[index_count] = Gyro_axis_data_f[0];
noname001 1:235bfa26c5c1 333 gyro_init_2[index_count] = Gyro_axis_data_f[1];
noname001 1:235bfa26c5c1 334 gyro_init_3[index_count] = Gyro_axis_data_f[2];
noname001 1:235bfa26c5c1 335 index_count++;
noname001 1:235bfa26c5c1 336 }
noname001 1:235bfa26c5c1 337 else if(gyro_count == 580)
noname001 1:235bfa26c5c1 338 {
noname001 1:235bfa26c5c1 339 gyro_init_1[index_count] = Gyro_axis_data_f[0];
noname001 1:235bfa26c5c1 340 gyro_init_2[index_count] = Gyro_axis_data_f[1];
noname001 1:235bfa26c5c1 341 gyro_init_3[index_count] = Gyro_axis_data_f[2];
noname001 1:235bfa26c5c1 342 index_count++;
noname001 1:235bfa26c5c1 343 }
noname001 1:235bfa26c5c1 344 else if(gyro_count >= 590)
noname001 1:235bfa26c5c1 345 {
noname001 1:235bfa26c5c1 346 gyro_init_1[index_count] = Gyro_axis_data_f[0];
noname001 1:235bfa26c5c1 347 gyro_init_2[index_count] = Gyro_axis_data_f[1];
noname001 1:235bfa26c5c1 348 gyro_init_3[index_count] = Gyro_axis_data_f[2];
noname001 1:235bfa26c5c1 349 z = 0;
noname001 1:235bfa26c5c1 350 for(z=0 ; z<10 ; z++)
noname001 1:235bfa26c5c1 351 {
noname001 1:235bfa26c5c1 352 gyro_sum_1 = gyro_sum_1 + gyro_init_1[z];
noname001 1:235bfa26c5c1 353 gyro_sum_2 = gyro_sum_2 + gyro_init_2[z];
noname001 1:235bfa26c5c1 354 gyro_sum_3 = gyro_sum_3 + gyro_init_3[z];
noname001 1:235bfa26c5c1 355 }
noname001 1:235bfa26c5c1 356
noname001 1:235bfa26c5c1 357 Gyro_axis_zero[0] = gyro_sum_1 / 10;
noname001 1:235bfa26c5c1 358 Gyro_axis_zero[1] = gyro_sum_2 / 10;
noname001 1:235bfa26c5c1 359 Gyro_axis_zero[2] = gyro_sum_3 / 10;
noname001 1:235bfa26c5c1 360 index_count = 0;
noname001 1:235bfa26c5c1 361 gyro_count = 0;
noname001 1:235bfa26c5c1 362 gyro_flag = 1;
noname001 1:235bfa26c5c1 363 }
noname001 1:235bfa26c5c1 364 }
noname001 1:235bfa26c5c1 365 //831 2:20
noname001 1:235bfa26c5c1 366 // acce filter
noname001 1:235bfa26c5c1 367 Acce_axis_data_f[0] = lpf(Acce_axis_data[0],Acce_axis_data_f_old[0],18);
noname001 1:235bfa26c5c1 368 Acce_axis_data_f_old[0] = Acce_axis_data_f[0];
noname001 1:235bfa26c5c1 369 Acce_axis_data_f[1] = lpf(Acce_axis_data[1],Acce_axis_data_f_old[1],18);
noname001 1:235bfa26c5c1 370 Acce_axis_data_f_old[1] = Acce_axis_data_f[1];
noname001 1:235bfa26c5c1 371 Acce_axis_data_f[2] = lpf(Acce_axis_data[2],Acce_axis_data_f_old[2],18);
noname001 1:235bfa26c5c1 372 Acce_axis_data_f_old[2] = Acce_axis_data_f[2];
noname001 1:235bfa26c5c1 373
noname001 1:235bfa26c5c1 374 Gyro_scale[0] = (Gyro_axis_data_f[0]-Gyro_axis_zero[0])*Gyro_gain_x; //88888
noname001 1:235bfa26c5c1 375 Gyro_scale[1] = (Gyro_axis_data_f[1]-Gyro_axis_zero[1])*Gyro_gain_y;
noname001 1:235bfa26c5c1 376 Gyro_scale[2] = (Gyro_axis_data_f[2]-Gyro_axis_zero[2])*Gyro_gain_z;
noname001 1:235bfa26c5c1 377
noname001 1:235bfa26c5c1 378 Acce_scale[0] = ((Acce_axis_data_f[0]-Acce_axis_zero[0]))*Acce_gainx;
noname001 1:235bfa26c5c1 379 Acce_scale[1] = ((Acce_axis_data_f[1]-Acce_axis_zero[1]))*Acce_gainy;
noname001 1:235bfa26c5c1 380 Acce_scale[2] = ((Acce_axis_data_f[2]-Acce_axis_zero[2]))*Acce_gainz;
noname001 1:235bfa26c5c1 381
noname001 1:235bfa26c5c1 382 // final 6 axis data
noname001 1:235bfa26c5c1 383 axm = Acce_scale[0];
noname001 1:235bfa26c5c1 384 aym = Acce_scale[1];
noname001 1:235bfa26c5c1 385 azm = Acce_scale[2];
noname001 1:235bfa26c5c1 386
noname001 1:235bfa26c5c1 387 u1 = Gyro_scale[0];
noname001 1:235bfa26c5c1 388 u2 = Gyro_scale[1];
noname001 1:235bfa26c5c1 389 u3 = Gyro_scale[2];
noname001 1:235bfa26c5c1 390
noname001 1:235bfa26c5c1 391 // new defined direction
noname001 1:235bfa26c5c1 392 ax = -aym;
noname001 1:235bfa26c5c1 393 ay = axm;
noname001 1:235bfa26c5c1 394 az = azm;
noname001 1:235bfa26c5c1 395
noname001 1:235bfa26c5c1 396 w1 = -u2;
noname001 1:235bfa26c5c1 397 w2 = u1;
noname001 1:235bfa26c5c1 398 w3 = u3;
noname001 1:235bfa26c5c1 399
noname001 1:235bfa26c5c1 400 // w deadzone
noname001 1:235bfa26c5c1 401 flag_d_1 = 0;
noname001 1:235bfa26c5c1 402 flag_d_2 = 0;
noname001 1:235bfa26c5c1 403 flag_d_1 = w1 <= 0.015f;
noname001 1:235bfa26c5c1 404 flag_d_2 = w1 >= -0.015f;
noname001 1:235bfa26c5c1 405 if((flag_d_1 + flag_d_2) == 2)
noname001 1:235bfa26c5c1 406 w1_d = 0;
noname001 1:235bfa26c5c1 407 else
noname001 1:235bfa26c5c1 408 w1_d = w1;
noname001 1:235bfa26c5c1 409
noname001 1:235bfa26c5c1 410 flag_d_3 = 0;
noname001 1:235bfa26c5c1 411 flag_d_4 = 0;
noname001 1:235bfa26c5c1 412 flag_d_3 = w2 <= 0.015f;
noname001 1:235bfa26c5c1 413 flag_d_4 = w2 >= -0.015f;
noname001 1:235bfa26c5c1 414 if((flag_d_3 + flag_d_4) == 2)
noname001 1:235bfa26c5c1 415 w2_d = 0;
noname001 1:235bfa26c5c1 416 else
noname001 1:235bfa26c5c1 417 w2_d = w2;
noname001 1:235bfa26c5c1 418
noname001 1:235bfa26c5c1 419 flag_d_5 = 0;
noname001 1:235bfa26c5c1 420 flag_d_6 = 0;
noname001 1:235bfa26c5c1 421 flag_d_5 = w3 <= 0.015f;
noname001 1:235bfa26c5c1 422 flag_d_6 = w3 >= -0.015f;
noname001 1:235bfa26c5c1 423 if((flag_d_5 + flag_d_6) == 2)
noname001 1:235bfa26c5c1 424 w3_d = 0;
noname001 1:235bfa26c5c1 425 else
noname001 1:235bfa26c5c1 426 w3_d = w3;
noname001 1:235bfa26c5c1 427 }
noname001 1:235bfa26c5c1 428
noname001 1:235bfa26c5c1 429 // velocity filter
noname001 1:235bfa26c5c1 430 v_f = lpf(v,v_f_old,18);
noname001 1:235bfa26c5c1 431 v_f_old = v_f;
noname001 1:235bfa26c5c1 432
noname001 1:235bfa26c5c1 433 // 離心加速度
noname001 1:235bfa26c5c1 434 acy_hat = 1.4f*v_f*w1_d; // 1.2
noname001 1:235bfa26c5c1 435 acy_hat_f = lpf(acy_hat,acy_hat_f_old,18);
noname001 1:235bfa26c5c1 436 acy_hat_f_old = acy_hat_f;
noname001 1:235bfa26c5c1 437
noname001 1:235bfa26c5c1 438 // sensor fusion by backward Euler method
noname001 1:235bfa26c5c1 439 gs1_hat = (gs1_hat_old + T*(-w2_d*alpf_hat + p*ax - w3_d*(acy_hat_f-ay) - w2_d*az)) / (1+p*T);
noname001 1:235bfa26c5c1 440 gs1_hat_old = gs1_hat;
noname001 1:235bfa26c5c1 441
noname001 1:235bfa26c5c1 442 gs2_hat = (gs2_hat_old + T*(w1_d*alpf_hat - w3_d*ax - p*(acy_hat_f-ay) + w1_d*az)) / (1+p*T);
noname001 1:235bfa26c5c1 443 gs2_hat_old = gs2_hat;
noname001 1:235bfa26c5c1 444
noname001 1:235bfa26c5c1 445 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);
noname001 1:235bfa26c5c1 446 gs3_hat_old = gs3_hat;
noname001 1:235bfa26c5c1 447
noname001 1:235bfa26c5c1 448 alpf_hat = (alpf_hat_old + T*((a+d)*gs3_hat + e*v_hat - (a+d)*az - e*v_f)) / (1+(a+d)*T);
noname001 1:235bfa26c5c1 449 alpf_hat_old = alpf_hat;
noname001 1:235bfa26c5c1 450
noname001 1:235bfa26c5c1 451 v_hat = (v_hat_old + T*((1+f)*gs3_hat - f*alpf_hat - (1+f)*az - g*v_f)) / (1-g*T);
noname001 1:235bfa26c5c1 452 v_hat_old = v_hat;
noname001 1:235bfa26c5c1 453
noname001 1:235bfa26c5c1 454 /*
noname001 1:235bfa26c5c1 455 // 離心加速度
noname001 1:235bfa26c5c1 456 acy_hat = 1.2*v_f*w1;
noname001 1:235bfa26c5c1 457 acy_hat_f = lpf(acy_hat,acy_hat_f_old,18);
noname001 1:235bfa26c5c1 458 acy_hat_f_old = acy_hat_f;
noname001 1:235bfa26c5c1 459
noname001 1:235bfa26c5c1 460 // sensor fusion by backward Euler method
noname001 1:235bfa26c5c1 461 gs1_hat = (gs1_hat_old + T*(-w2*alpf_hat + p*ax - w3*(acy_hat_f-ay) - w2*az)) / (1+p*T);
noname001 1:235bfa26c5c1 462 gs1_hat_old = gs1_hat;
noname001 1:235bfa26c5c1 463
noname001 1:235bfa26c5c1 464 gs2_hat = (gs2_hat_old + T*(w1*alpf_hat - w3*ax - p*(acy_hat_f-ay) + w1*az)) / (1+p*T);
noname001 1:235bfa26c5c1 465 gs2_hat_old = gs2_hat;
noname001 1:235bfa26c5c1 466
noname001 1:235bfa26c5c1 467 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);
noname001 1:235bfa26c5c1 468 gs3_hat_old = gs3_hat;
noname001 1:235bfa26c5c1 469
noname001 1:235bfa26c5c1 470 alpf_hat = (alpf_hat_old + T*((a+d)*gs3_hat + e*v_hat - (a+d)*az - e*v_f)) / (1+(a+d)*T);
noname001 1:235bfa26c5c1 471 alpf_hat_old = alpf_hat;
noname001 1:235bfa26c5c1 472
noname001 1:235bfa26c5c1 473 v_hat = (v_hat_old + T*((1+f)*gs3_hat - f*alpf_hat - (1+f)*az - g*v_f)) / (1-g*T);
noname001 1:235bfa26c5c1 474 v_hat_old = v_hat;
noname001 1:235bfa26c5c1 475 */
noname001 1:235bfa26c5c1 476
noname001 1:235bfa26c5c1 477 // normalize gs1_hat , gs2_hat , gs3_hat to find pitch angle and roll angle
noname001 1:235bfa26c5c1 478 n = sqrt(gs1_hat*gs1_hat + gs2_hat*gs2_hat + gs3_hat*gs3_hat);
noname001 1:235bfa26c5c1 479
noname001 1:235bfa26c5c1 480 gs1_hat_n = (gs1_hat / n) * 9.81f;
noname001 1:235bfa26c5c1 481 gs2_hat_n = (gs2_hat / n) * 9.81f;
noname001 1:235bfa26c5c1 482 gs3_hat_n = (gs3_hat / n) * 9.81f;
noname001 1:235bfa26c5c1 483
noname001 1:235bfa26c5c1 484 psi = arcsin(-gs3_hat_n / 9.81f);
noname001 1:235bfa26c5c1 485 //theta = atan(-gs2_hat_n / gs1_hat_n);
noname001 1:235bfa26c5c1 486
noname001 1:235bfa26c5c1 487 // psi correction
noname001 1:235bfa26c5c1 488 psi_m = psi - (0); // after correction
noname001 1:235bfa26c5c1 489 psi_deg = 57.3f * psi_m;
noname001 1:235bfa26c5c1 490
noname001 1:235bfa26c5c1 491 // psi deadzone
noname001 1:235bfa26c5c1 492 flag_d_1 = 0;
noname001 1:235bfa26c5c1 493 flag_d_2 = 0;
noname001 1:235bfa26c5c1 494 flag_d_1 = psi_deg < 2.5f; // 在平地時 flag_d_1 = 1
noname001 1:235bfa26c5c1 495 flag_d_2 = psi_deg > -3; // 在平地時 flag_d_2 = 1
noname001 1:235bfa26c5c1 496
noname001 1:235bfa26c5c1 497 if((flag_d_1 + flag_d_2) == 2) // 在平地時 flag_d_1 + flag_d_2 = 2
noname001 1:235bfa26c5c1 498 psi_d = 0;
noname001 1:235bfa26c5c1 499 else
noname001 1:235bfa26c5c1 500 psi_d = psi_m;
noname001 0:0b8d14ed499f 501
noname001 1:235bfa26c5c1 502 // 去除負加速度
noname001 1:235bfa26c5c1 503 if(alpf_hat < 0)
noname001 1:235bfa26c5c1 504 alpf_c = 0;
noname001 1:235bfa26c5c1 505 else
noname001 1:235bfa26c5c1 506 alpf_c = alpf_hat;
noname001 1:235bfa26c5c1 507
noname001 1:235bfa26c5c1 508 // 速度為負時不考慮加速度
noname001 1:235bfa26c5c1 509 if(v_f < 0)
noname001 1:235bfa26c5c1 510 alpf_v = 0;
noname001 1:235bfa26c5c1 511 else
noname001 1:235bfa26c5c1 512 alpf_v = alpf_c;
noname001 1:235bfa26c5c1 513
noname001 1:235bfa26c5c1 514 // 加速度 deadzone
noname001 1:235bfa26c5c1 515 if(alpf_v <= 0.1f)
noname001 1:235bfa26c5c1 516 alpf_d = 0;
noname001 1:235bfa26c5c1 517 else
noname001 1:235bfa26c5c1 518 alpf_d = alpf_v;
noname001 1:235bfa26c5c1 519
noname001 1:235bfa26c5c1 520 // 改button //improve //設定一個開關
noname001 1:235bfa26c5c1 521 /*
noname001 1:235bfa26c5c1 522 // buttom filter
noname001 1:235bfa26c5c1 523 buttom_buf = lpf(ADCBUF0,buttom_buf_old,30);
noname001 1:235bfa26c5c1 524 buttom_buf_old = buttom_buf;
noname001 1:235bfa26c5c1 525
noname001 1:235bfa26c5c1 526 // button control compensationrate
noname001 1:235bfa26c5c1 527 if(buttom_buf >= 450)
noname001 1:235bfa26c5c1 528 compensationrate = 1;
noname001 1:235bfa26c5c1 529 else
noname001 1:235bfa26c5c1 530 compensationrate = 0;
noname001 1:235bfa26c5c1 531 */
noname001 1:235bfa26c5c1 532
noname001 1:235bfa26c5c1 533 if(adc_switch.read() > 0.6f)
noname001 1:235bfa26c5c1 534 {
noname001 1:235bfa26c5c1 535 buttom_buf=1;
noname001 1:235bfa26c5c1 536 compensationrate = 1;
noname001 1:235bfa26c5c1 537 }
noname001 1:235bfa26c5c1 538 else
noname001 1:235bfa26c5c1 539 {
noname001 1:235bfa26c5c1 540 buttom_buf=0;
noname001 1:235bfa26c5c1 541 compensationrate = 0;
noname001 1:235bfa26c5c1 542 }
noname001 1:235bfa26c5c1 543
noname001 1:235bfa26c5c1 544 // 電流命令 (T = k*i)
noname001 1:235bfa26c5c1 545 current_command = compensationrate * (Mb*alpf_d*1.1f + M*9.81f*sinr(psi_d)*1.35f + Df) * (R/Ki);
noname001 1:235bfa26c5c1 546
noname001 1:235bfa26c5c1 547 // 電流命令上下限
noname001 1:235bfa26c5c1 548 if(current_command >= 15)
noname001 1:235bfa26c5c1 549 current_command = 15;
noname001 1:235bfa26c5c1 550 else if(current_command <= -25)
noname001 1:235bfa26c5c1 551 current_command = -25;
noname001 1:235bfa26c5c1 552 //831 412
noname001 1:235bfa26c5c1 553 // 開關安全措施
noname001 1:235bfa26c5c1 554 b_count = b_count + 1;
noname001 1:235bfa26c5c1 555
noname001 1:235bfa26c5c1 556 if(b_count >= 1500) // wait for buttom ADC buffer ready
noname001 1:235bfa26c5c1 557 {
noname001 1:235bfa26c5c1 558 b_init_flag = 1;
noname001 1:235bfa26c5c1 559 b_count = 0;
noname001 1:235bfa26c5c1 560 }
noname001 1:235bfa26c5c1 561
noname001 1:235bfa26c5c1 562 if(b_init_flag == 0)
noname001 1:235bfa26c5c1 563 open_flag = 1;
noname001 1:235bfa26c5c1 564 else if(b_init_flag == 1)
noname001 1:235bfa26c5c1 565 {
noname001 1:235bfa26c5c1 566 if(compensationrate == 1 && open_flag == 1) // 初始 open_flag = 1
noname001 1:235bfa26c5c1 567 protect_flag = 1;
noname001 1:235bfa26c5c1 568
noname001 1:235bfa26c5c1 569 if(compensationrate == 0) // 初始 open_flag = 1 , 等 compensationrate = 0 時才將 open_flag = 0
noname001 1:235bfa26c5c1 570 {
noname001 1:235bfa26c5c1 571 protect_flag = 0; // 初始 compensationrate = 0 時才將 protect_flag = 0
noname001 1:235bfa26c5c1 572 open_flag = 0;
noname001 1:235bfa26c5c1 573 b_flag = 0;
noname001 1:235bfa26c5c1 574 }
noname001 1:235bfa26c5c1 575 // 要先有初始 compensationrate = 0 才能先將 protect_flag == 0 && open_flag == 0 成立
noname001 1:235bfa26c5c1 576 // 之後由 compensationrate = 0 轉變成 compensationrate = 1 時才將 b_flag = 1(開始送命令)
noname001 1:235bfa26c5c1 577 if(compensationrate == 1 && protect_flag == 0 && open_flag == 0)
noname001 1:235bfa26c5c1 578 {
noname001 1:235bfa26c5c1 579 b_flag = 1;
noname001 1:235bfa26c5c1 580 }
noname001 1:235bfa26c5c1 581 }
noname001 1:235bfa26c5c1 582
noname001 1:235bfa26c5c1 583 // 改CAN
noname001 1:235bfa26c5c1 584 // can 傳資料到大板子
noname001 1:235bfa26c5c1 585 //C1TX0DLCbits.DLC = 8;
noname001 1:235bfa26c5c1 586 /*
noname001 1:235bfa26c5c1 587 if(b_flag == 1)
noname001 1:235bfa26c5c1 588 {
noname001 1:235bfa26c5c1 589 C1TX0B1 = current_command;
noname001 1:235bfa26c5c1 590 C1TX0B2 = compensationrate;
noname001 1:235bfa26c5c1 591 C1TX0B3 = 300;
noname001 1:235bfa26c5c1 592 }
noname001 1:235bfa26c5c1 593 else if(b_flag == 0)
noname001 1:235bfa26c5c1 594 {
noname001 1:235bfa26c5c1 595 C1TX0B1 = 0;
noname001 1:235bfa26c5c1 596 C1TX0B2 = 0;
noname001 1:235bfa26c5c1 597 C1TX0B3 = 300;
noname001 1:235bfa26c5c1 598 }
noname001 1:235bfa26c5c1 599 */
noname001 1:235bfa26c5c1 600 //C1TX0CONbits.TXREQ = 1;
noname001 1:235bfa26c5c1 601
noname001 1:235bfa26c5c1 602 // uart傳送資料
noname001 1:235bfa26c5c1 603
noname001 1:235bfa26c5c1 604 display[0] = 100*psi_deg;
noname001 1:235bfa26c5c1 605 display[1] = 100*gs2_hat;
noname001 1:235bfa26c5c1 606 display[2] = 100*gs3_hat;
noname001 1:235bfa26c5c1 607 display[3] = buttom_buf;
noname001 1:235bfa26c5c1 608 display[4] = current_command;
noname001 1:235bfa26c5c1 609 display[5] = rotationspeed;
noname001 1:235bfa26c5c1 610
noname001 1:235bfa26c5c1 611
noname001 0:0b8d14ed499f 612 separate();
noname001 1:235bfa26c5c1 613
noname001 0:0b8d14ed499f 614 j = 0;
noname001 0:0b8d14ed499f 615 k = 1;
noname001 1:235bfa26c5c1 616 while(k)
noname001 1:235bfa26c5c1 617 {
noname001 1:235bfa26c5c1 618 if(uart_1.writeable())
noname001 1:235bfa26c5c1 619 {
noname001 0:0b8d14ed499f 620 uart_1.putc(num[i]);
noname001 0:0b8d14ed499f 621 i++;
noname001 0:0b8d14ed499f 622 j++;
noname001 0:0b8d14ed499f 623 }
noname001 0:0b8d14ed499f 624
noname001 1:235bfa26c5c1 625 if(j>1) // send 2 bytes
noname001 1:235bfa26c5c1 626 {
noname001 0:0b8d14ed499f 627 k = 0;
noname001 0:0b8d14ed499f 628 j = 0;
noname001 0:0b8d14ed499f 629 }
noname001 0:0b8d14ed499f 630 }
noname001 0:0b8d14ed499f 631
noname001 0:0b8d14ed499f 632 if(i>13)
noname001 0:0b8d14ed499f 633 i = 0;
noname001 1:235bfa26c5c1 634
noname001 1:235bfa26c5c1 635 //LATDbits.LATD3 = 0;
noname001 1:235bfa26c5c1 636
noname001 1:235bfa26c5c1 637 //IFS0bits.T1IF = 0; // 重置中斷旗標
noname001 0:0b8d14ed499f 638 }
noname001 1:235bfa26c5c1 639
noname001 1:235bfa26c5c1 640 void init_uart()
noname001 1:235bfa26c5c1 641 {
noname001 1:235bfa26c5c1 642 uart_1.baud(115200); // 設定baudrate
noname001 1:235bfa26c5c1 643 }
noname001 1:235bfa26c5c1 644
noname001 1:235bfa26c5c1 645 void separate(void)
noname001 1:235bfa26c5c1 646 {
noname001 1:235bfa26c5c1 647 num[2] = display[0] >> 8; // HSB(8bit)資料存入陣列
noname001 1:235bfa26c5c1 648 num[3] = display[0] & 0x00ff; // LSB(8bit)資料存入陣列
noname001 1:235bfa26c5c1 649 num[4] = display[1] >> 8;
noname001 1:235bfa26c5c1 650 num[5] = display[1] & 0x00ff;
noname001 1:235bfa26c5c1 651 num[6] = display[2] >> 8;
noname001 1:235bfa26c5c1 652 num[7] = display[2] & 0x00ff;
noname001 1:235bfa26c5c1 653 num[8] = display[3] >> 8;
noname001 1:235bfa26c5c1 654 num[9] = display[3] & 0x00ff;
noname001 1:235bfa26c5c1 655 num[10] = display[4] >> 8;
noname001 1:235bfa26c5c1 656 num[11] = display[4] & 0x00ff;
noname001 1:235bfa26c5c1 657 num[12] = display[5] >> 8;
noname001 1:235bfa26c5c1 658 num[13] = display[5] & 0x00ff;
noname001 1:235bfa26c5c1 659 }
noname001 1:235bfa26c5c1 660
noname001 1:235bfa26c5c1 661 /*
noname001 1:235bfa26c5c1 662 void init_spi(void)
noname001 1:235bfa26c5c1 663 {
noname001 1:235bfa26c5c1 664 SPI1STATbits.SPISIDL = 1; // Stop in Idle Mode bit
noname001 1:235bfa26c5c1 665 SPI1CONbits.FRMEN = 0; // Framed SPI support disabled
noname001 1:235bfa26c5c1 666 SPI1CONbits.DISSDO = 0; // SDO1 pin is controlled by the module
noname001 1:235bfa26c5c1 667 SPI1CONbits.MODE16 = 0; // Communication is byte-wide (8 bits)
noname001 1:235bfa26c5c1 668 SPI1CONbits.SMP = 0; // Input data sampled at middle of data output time
noname001 1:235bfa26c5c1 669 SPI1CONbits.CKE = 0; // Serial output data changes on transition from Idle clock state to active clock state
noname001 1:235bfa26c5c1 670 SPI1CONbits.CKP = 1; // Idle state for clock is a high level; active state is a low level
noname001 1:235bfa26c5c1 671 SPI1CONbits.SSEN = 0; // Slave Select Unable
noname001 1:235bfa26c5c1 672 SPI1CONbits.MSTEN = 1; // Master mode
noname001 1:235bfa26c5c1 673 // Fsck = Fcy / (Secondary Prescale * Primary Prescale)
noname001 1:235bfa26c5c1 674 SPI1CONbits.SPRE = 0b111; // Secondary Prescale (Master Mode) bits , 111 = Secondary prescale 1:1
noname001 1:235bfa26c5c1 675 SPI1CONbits.PPRE = 0b01; // Primary Prescale (Master Mode) bits , 01 = Primary prescale 16:1
noname001 1:235bfa26c5c1 676
noname001 1:235bfa26c5c1 677 // SPI enable 最後才能打開
noname001 1:235bfa26c5c1 678 SPI1STATbits.SPIEN = 1; // Enables module and configures SCK1, SDO1, SDI1 and SS1 as serial port pins
noname001 1:235bfa26c5c1 679 }
noname001 1:235bfa26c5c1 680 */
noname001 1:235bfa26c5c1 681
noname001 1:235bfa26c5c1 682 void init_Sensors(void)
noname001 1:235bfa26c5c1 683 {
noname001 1:235bfa26c5c1 684 sensor.begin();
noname001 1:235bfa26c5c1 685 // sensor.begin() setting :
noname001 1:235bfa26c5c1 686 // uint16_t begin(gyro_scale gScl = G_SCALE_2000DPS,
noname001 1:235bfa26c5c1 687 // accel_scale aScl = A_SCALE_8G,
noname001 1:235bfa26c5c1 688 // mag_scale mScl = M_SCALE_8GS,
noname001 1:235bfa26c5c1 689 // gyro_odr gODR = G_ODR_760_BW_100,
noname001 1:235bfa26c5c1 690 // accel_odr aODR = A_ODR_800,
noname001 1:235bfa26c5c1 691 // mag_odr mODR = M_ODR_100);
noname001 1:235bfa26c5c1 692
noname001 1:235bfa26c5c1 693 //sensor_CSXM = 1;
noname001 1:235bfa26c5c1 694 //sensor_CSG = 1;
noname001 1:235bfa26c5c1 695 //Gyro_axis_data[0] = 0; // X
noname001 1:235bfa26c5c1 696 //Gyro_axis_data[1] = 0; // Y
noname001 1:235bfa26c5c1 697 //Gyro_axis_data[2] = 0; // Z
noname001 1:235bfa26c5c1 698 //Acce_axis_data[0] = 0;
noname001 1:235bfa26c5c1 699 //Acce_axis_data[1] = 0;
noname001 1:235bfa26c5c1 700 //Acce_axis_data[2] = 0;
noname001 1:235bfa26c5c1 701 //sensorG_setup();
noname001 1:235bfa26c5c1 702 //sensorXM_setup();
noname001 1:235bfa26c5c1 703 }
noname001 1:235bfa26c5c1 704 /*
noname001 1:235bfa26c5c1 705 void init_sensor(void)
noname001 1:235bfa26c5c1 706 {
noname001 1:235bfa26c5c1 707 sensor.begin();
noname001 1:235bfa26c5c1 708 // sensor.begin() setting :
noname001 1:235bfa26c5c1 709 // uint16_t begin(gyro_scale gScl = G_SCALE_2000DPS,
noname001 1:235bfa26c5c1 710 // accel_scale aScl = A_SCALE_8G,
noname001 1:235bfa26c5c1 711 // mag_scale mScl = M_SCALE_8GS,
noname001 1:235bfa26c5c1 712 // gyro_odr gODR = G_ODR_760_BW_100,
noname001 1:235bfa26c5c1 713 // accel_odr aODR = A_ODR_800,
noname001 1:235bfa26c5c1 714 // mag_odr mODR = M_ODR_100);
noname001 1:235bfa26c5c1 715 }
noname001 1:235bfa26c5c1 716 */
noname001 1:235bfa26c5c1 717 /*
noname001 1:235bfa26c5c1 718 unsigned char SPI_operation(unsigned char data_send) // 具有 write 跟 read 兩種功能
noname001 1:235bfa26c5c1 719 {
noname001 1:235bfa26c5c1 720 static unsigned char data_recived = 0;
noname001 1:235bfa26c5c1 721
noname001 1:235bfa26c5c1 722 while(SPI1STATbits.SPITBF==1); // wait for SPI1STATbits.SPITBF = 0 (transmit start , SPI1TXB is empty)
noname001 1:235bfa26c5c1 723 SPI1BUF = data_send; // write SPI
noname001 1:235bfa26c5c1 724
noname001 1:235bfa26c5c1 725 while(SPI1STATbits.SPIRBF==0); // wait for SPI1STATbits.SPIRBF = 1 (receive complete , SPI1RXB is full)
noname001 1:235bfa26c5c1 726 data_recived = SPI1BUF; // read SPI
noname001 1:235bfa26c5c1 727
noname001 1:235bfa26c5c1 728 return data_recived;
noname001 1:235bfa26c5c1 729 }
noname001 1:235bfa26c5c1 730 */
noname001 1:235bfa26c5c1 731 /*
noname001 1:235bfa26c5c1 732 void sensorG_setup(void)
noname001 1:235bfa26c5c1 733 {
noname001 1:235bfa26c5c1 734 static unsigned char sensorG_data_write;
noname001 1:235bfa26c5c1 735 static unsigned char sensorG_data_read;
noname001 1:235bfa26c5c1 736
noname001 1:235bfa26c5c1 737 // 設定暫存器的值
noname001 1:235bfa26c5c1 738 sensorG_CTRL_REG1 = 0b11111111; // ODR = 760Hz , BW = 100Hz
noname001 1:235bfa26c5c1 739 // Power-down mode is normal mode , XYZ enable
noname001 1:235bfa26c5c1 740 sensorG_CTRL_REG2 = 0b00001001; // High-pass filter mode is normal mode
noname001 1:235bfa26c5c1 741 // High-pass filter cutoff frequency = 0.09Hz
noname001 1:235bfa26c5c1 742 sensorG_CTRL_REG3 = 0b00000000; // no use , all 0
noname001 1:235bfa26c5c1 743 sensorG_CTRL_REG4 = 0b00110000; // 2000dps
noname001 1:235bfa26c5c1 744 sensorG_CTRL_REG5 = 0b00000000; // no use , all 0
noname001 1:235bfa26c5c1 745
noname001 1:235bfa26c5c1 746 // 設定暫存器的位址
noname001 1:235bfa26c5c1 747 // write mode (第8個bit = 0)
noname001 1:235bfa26c5c1 748 // auto increase address (第7個bit = 1)
noname001 1:235bfa26c5c1 749 sensorG_data_write = 0x20; // sensor_G_CTRL address
noname001 1:235bfa26c5c1 750 sensorG_data_write = (sensorG_data_write & 0b00111111) | 0b01000000;
noname001 1:235bfa26c5c1 751 // mask first two bits : sensorG_CTRL_REG1_address & 0b00111111 , 把最前面兩個bit清0 , 其他bit保留不動
noname001 1:235bfa26c5c1 752 // write first two bits : (...) | 0b01000000 把最前面兩個bit寫成01 , 其他bit保留不動
noname001 1:235bfa26c5c1 753
noname001 1:235bfa26c5c1 754 // open
noname001 1:235bfa26c5c1 755 sensor_CSG = 0;
noname001 1:235bfa26c5c1 756
noname001 1:235bfa26c5c1 757 // CS打開後 , 會先寫暫存器的位址 , 再寫暫存器的值
noname001 1:235bfa26c5c1 758 // 寫入暫存器的位址後就完成write mode和auto increase address的功能
noname001 1:235bfa26c5c1 759 // 接下來就會依序遞增暫存器的位址並做寫入的動作
noname001 1:235bfa26c5c1 760 // start to write register address
noname001 1:235bfa26c5c1 761 sensorG_data_read = SPI_operation(sensorG_data_write);
noname001 1:235bfa26c5c1 762
noname001 1:235bfa26c5c1 763 // start to set register value
noname001 1:235bfa26c5c1 764 sensorG_data_read = SPI_operation(sensorG_CTRL_REG1);
noname001 1:235bfa26c5c1 765 sensorG_data_read = SPI_operation(sensorG_CTRL_REG2);
noname001 1:235bfa26c5c1 766 sensorG_data_read = SPI_operation(sensorG_CTRL_REG3);
noname001 1:235bfa26c5c1 767 sensorG_data_read = SPI_operation(sensorG_CTRL_REG4);
noname001 1:235bfa26c5c1 768 sensorG_data_read = SPI_operation(sensorG_CTRL_REG5);
noname001 1:235bfa26c5c1 769
noname001 1:235bfa26c5c1 770 // close
noname001 1:235bfa26c5c1 771 sensor_CSG = 1;
noname001 1:235bfa26c5c1 772 }
noname001 1:235bfa26c5c1 773 */
noname001 1:235bfa26c5c1 774 /*
noname001 1:235bfa26c5c1 775 void sensorXM_setup(void)
noname001 1:235bfa26c5c1 776 {
noname001 1:235bfa26c5c1 777 static unsigned char sensorXM_data_write;
noname001 1:235bfa26c5c1 778 static unsigned char sensorXM_data_read;
noname001 1:235bfa26c5c1 779
noname001 1:235bfa26c5c1 780 // 設定暫存器的值
noname001 1:235bfa26c5c1 781 sensorXM_CTRL_REG0 = 0b00000000; // no use , all 0
noname001 1:235bfa26c5c1 782 sensorXM_CTRL_REG1 = 0b10010111; // ODR = 800Hz , XYZ enable
noname001 1:235bfa26c5c1 783 sensorXM_CTRL_REG2 = 0b11011000; // anti-alias filter BW = 50Hz
noname001 1:235bfa26c5c1 784 // acceleration full scale = +-8 g
noname001 1:235bfa26c5c1 785 sensorXM_CTRL_REG3 = 0b00000000; // no use , all 0
noname001 1:235bfa26c5c1 786 sensorXM_CTRL_REG4 = 0b00000000; // no use , all 0
noname001 1:235bfa26c5c1 787 sensorXM_CTRL_REG5 = 0b01110100; // magnetic resolution is high resolution
noname001 1:235bfa26c5c1 788 // Power mode selection = 100Hz
noname001 1:235bfa26c5c1 789 sensorXM_CTRL_REG6 = 0b01000000; // Magnetic full scale = +-8 gauss
noname001 1:235bfa26c5c1 790 sensorXM_CTRL_REG7 = 0b00000000; // no use , all 0
noname001 1:235bfa26c5c1 791
noname001 1:235bfa26c5c1 792 // 設定暫存器的位址
noname001 1:235bfa26c5c1 793 // write mode (第8個bit = 0)
noname001 1:235bfa26c5c1 794 // auto increase address (第7個bit = 1)
noname001 1:235bfa26c5c1 795 sensorXM_data_write = 0x1F; // sensor_XM_CTRL address
noname001 1:235bfa26c5c1 796 sensorXM_data_write = (sensorXM_data_write & 0b00111111) | 0b01000000;
noname001 1:235bfa26c5c1 797 // mask first two bits , write first two bits
noname001 1:235bfa26c5c1 798
noname001 1:235bfa26c5c1 799 // open
noname001 1:235bfa26c5c1 800 sensor_CSXM = 0;
noname001 1:235bfa26c5c1 801
noname001 1:235bfa26c5c1 802 // start to write register address
noname001 1:235bfa26c5c1 803 sensorXM_data_read = SPI_operation(sensorXM_data_write);
noname001 1:235bfa26c5c1 804
noname001 1:235bfa26c5c1 805 // start to set register value
noname001 1:235bfa26c5c1 806 sensorXM_data_read = SPI_operation(sensorXM_CTRL_REG0);
noname001 1:235bfa26c5c1 807 sensorXM_data_read = SPI_operation(sensorXM_CTRL_REG1);
noname001 1:235bfa26c5c1 808 sensorXM_data_read = SPI_operation(sensorXM_CTRL_REG2);
noname001 1:235bfa26c5c1 809 sensorXM_data_read = SPI_operation(sensorXM_CTRL_REG3);
noname001 1:235bfa26c5c1 810 sensorXM_data_read = SPI_operation(sensorXM_CTRL_REG4);
noname001 1:235bfa26c5c1 811 sensorXM_data_read = SPI_operation(sensorXM_CTRL_REG5);
noname001 1:235bfa26c5c1 812 sensorXM_data_read = SPI_operation(sensorXM_CTRL_REG6);
noname001 1:235bfa26c5c1 813 sensorXM_data_read = SPI_operation(sensorXM_CTRL_REG7);
noname001 1:235bfa26c5c1 814
noname001 1:235bfa26c5c1 815 // close
noname001 1:235bfa26c5c1 816 sensor_CSXM = 1;
noname001 1:235bfa26c5c1 817 }
noname001 1:235bfa26c5c1 818 */
noname001 1:235bfa26c5c1 819
noname001 1:235bfa26c5c1 820 /*
noname001 1:235bfa26c5c1 821 void sensorG_read_3axis(void)
noname001 1:235bfa26c5c1 822 {
noname001 1:235bfa26c5c1 823 static unsigned char sensorG_data_write;
noname001 1:235bfa26c5c1 824 static int sensorG_data_read[6];
noname001 1:235bfa26c5c1 825
noname001 1:235bfa26c5c1 826 // 設定暫存器的位址
noname001 1:235bfa26c5c1 827 // read mode (第8個bit = 1)
noname001 1:235bfa26c5c1 828 // auto increase address (第7個bit = 1)
noname001 1:235bfa26c5c1 829 sensorG_data_write = 0x28; // gyroscope's address
noname001 1:235bfa26c5c1 830 sensorG_data_write = (sensorG_data_write & 0b00111111) | 0b11000000;
noname001 1:235bfa26c5c1 831 // mask first two bits , write first two bits
noname001 1:235bfa26c5c1 832 // 因為暫存器位址只有6個bit , 所以用mask和write來設定read mode , auto increase address
noname001 1:235bfa26c5c1 833 // 不會改變到暫存器位址的值
noname001 1:235bfa26c5c1 834
noname001 1:235bfa26c5c1 835 // open
noname001 1:235bfa26c5c1 836 sensor_CSG = 0;
noname001 1:235bfa26c5c1 837
noname001 1:235bfa26c5c1 838 // CS打開後 , 會先寫暫存器的位址 , 再寫暫存器的值
noname001 1:235bfa26c5c1 839 // 寫入暫存器的位址後就完成read mode和auto increase address的功能
noname001 1:235bfa26c5c1 840 // 接下來就會依序遞增暫存器的位址並做讀取的動作
noname001 1:235bfa26c5c1 841 // start to write register address
noname001 1:235bfa26c5c1 842 sensorG_data_read[0] = (int)SPI_operation(sensorG_data_write);
noname001 1:235bfa26c5c1 843
noname001 1:235bfa26c5c1 844 // start to read register value
noname001 1:235bfa26c5c1 845 // gyroscope output
noname001 1:235bfa26c5c1 846 // 因為這邊的SPI函數是用來讀值 , 而SPI函數是把寫和讀功能寫在一起的函數
noname001 1:235bfa26c5c1 847 // 所以寫入的值就任意設成0x00 , 反正寫入的值不重要
noname001 1:235bfa26c5c1 848 sensorG_data_read[0] = (int)SPI_operation(0x00); // XL
noname001 1:235bfa26c5c1 849 sensorG_data_read[1] = (int)SPI_operation(0x00); // XH
noname001 1:235bfa26c5c1 850 sensorG_data_read[2] = (int)SPI_operation(0x00); // YL
noname001 1:235bfa26c5c1 851 sensorG_data_read[3] = (int)SPI_operation(0x00); // YH
noname001 1:235bfa26c5c1 852 sensorG_data_read[4] = (int)SPI_operation(0x00); // ZL
noname001 1:235bfa26c5c1 853 sensorG_data_read[5] = (int)SPI_operation(0x00); // ZH
noname001 1:235bfa26c5c1 854
noname001 1:235bfa26c5c1 855 // close
noname001 1:235bfa26c5c1 856 sensor_CSG = 1;
noname001 1:235bfa26c5c1 857
noname001 1:235bfa26c5c1 858 // data reconstruction
noname001 1:235bfa26c5c1 859 Gyro_axis_data[0] = (sensorG_data_read[1]<<8) | sensorG_data_read[0]; // X
noname001 1:235bfa26c5c1 860 Gyro_axis_data[1] = (sensorG_data_read[3]<<8) | sensorG_data_read[2]; // Y
noname001 1:235bfa26c5c1 861 Gyro_axis_data[2] = (sensorG_data_read[5]<<8) | sensorG_data_read[4]; // Z
noname001 1:235bfa26c5c1 862 }
noname001 1:235bfa26c5c1 863 */
noname001 1:235bfa26c5c1 864 /*
noname001 1:235bfa26c5c1 865 void sensorXM_read_3axis(void)
noname001 1:235bfa26c5c1 866 {
noname001 1:235bfa26c5c1 867 static unsigned char sensorXM_data_write;
noname001 1:235bfa26c5c1 868 static int sensorXM_data_read[6];
noname001 1:235bfa26c5c1 869
noname001 1:235bfa26c5c1 870 // 設定暫存器的位址
noname001 1:235bfa26c5c1 871 // read mode (第8個bit = 1)
noname001 1:235bfa26c5c1 872 // auto increase address (第7個bit = 1)
noname001 1:235bfa26c5c1 873 sensorXM_data_write = 0x28; // accelerometer's address
noname001 1:235bfa26c5c1 874 sensorXM_data_write = (sensorXM_data_write & 0b00111111) | 0b11000000;
noname001 1:235bfa26c5c1 875 // mask first two bits , write first two bits
noname001 1:235bfa26c5c1 876
noname001 1:235bfa26c5c1 877 // open
noname001 1:235bfa26c5c1 878 sensor_CSXM = 0;
noname001 1:235bfa26c5c1 879
noname001 1:235bfa26c5c1 880 // start to write register address
noname001 1:235bfa26c5c1 881 sensorXM_data_read[0] = (int)SPI_operation(sensorXM_data_write);
noname001 1:235bfa26c5c1 882
noname001 1:235bfa26c5c1 883 // start to read register value
noname001 1:235bfa26c5c1 884 // accelerometer output
noname001 1:235bfa26c5c1 885 sensorXM_data_read[0] = (int)SPI_operation(0x00); // XL
noname001 1:235bfa26c5c1 886 sensorXM_data_read[1] = (int)SPI_operation(0x00); // XH
noname001 1:235bfa26c5c1 887 sensorXM_data_read[2] = (int)SPI_operation(0x00); // YL
noname001 1:235bfa26c5c1 888 sensorXM_data_read[3] = (int)SPI_operation(0x00); // YH
noname001 1:235bfa26c5c1 889 sensorXM_data_read[4] = (int)SPI_operation(0x00); // ZL
noname001 1:235bfa26c5c1 890 sensorXM_data_read[5] = (int)SPI_operation(0x00); // ZH
noname001 1:235bfa26c5c1 891
noname001 1:235bfa26c5c1 892 // close
noname001 1:235bfa26c5c1 893 sensor_CSXM = 1;
noname001 1:235bfa26c5c1 894
noname001 1:235bfa26c5c1 895 // data reconstruction
noname001 1:235bfa26c5c1 896 Acce_axis_data[0] = (sensorXM_data_read[1]<<8) | sensorXM_data_read[0]; // X
noname001 1:235bfa26c5c1 897 Acce_axis_data[1] = (sensorXM_data_read[3]<<8) | sensorXM_data_read[2]; // Y
noname001 1:235bfa26c5c1 898 Acce_axis_data[2] = (sensorXM_data_read[5]<<8) | sensorXM_data_read[4]; // Z
noname001 1:235bfa26c5c1 899 }
noname001 1:235bfa26c5c1 900 */
noname001 1:235bfa26c5c1 901 /*
noname001 1:235bfa26c5c1 902 void init_can(void)
noname001 1:235bfa26c5c1 903 {
noname001 1:235bfa26c5c1 904 CAN1SetOperationMode( CAN_IDLE_CON & CAN_MASTERCLOCK_1 &
noname001 1:235bfa26c5c1 905 CAN_REQ_OPERMODE_CONFIG );
noname001 1:235bfa26c5c1 906
noname001 1:235bfa26c5c1 907 while ( C1CTRLbits.OPMODE <= 3 );
noname001 1:235bfa26c5c1 908
noname001 1:235bfa26c5c1 909 CAN1Initialize( CAN_SYNC_JUMP_WIDTH4 & CAN_BAUD_PRE_SCALE(2),
noname001 1:235bfa26c5c1 910 CAN_WAKEUP_BY_FILTER_DIS &
noname001 1:235bfa26c5c1 911 CAN_PHASE_SEG2_TQ(3) &
noname001 1:235bfa26c5c1 912 CAN_PHASE_SEG1_TQ(3) &
noname001 1:235bfa26c5c1 913 CAN_PROPAGATIONTIME_SEG_TQ(3) &
noname001 1:235bfa26c5c1 914 CAN_SEG2_TIME_LIMIT_SET & CAN_SAMPLE3TIMES ) ;
noname001 1:235bfa26c5c1 915
noname001 1:235bfa26c5c1 916 CAN1SetFilter ( (char) 0 , CAN_FILTER_SID( 0x200 ) & CAN_RX_EID_DIS ,
noname001 1:235bfa26c5c1 917 CAN_FILTER_EID( 0x00 ) );
noname001 1:235bfa26c5c1 918
noname001 1:235bfa26c5c1 919
noname001 1:235bfa26c5c1 920 CAN1SetMask ( (char)0 , CAN_MASK_SID ( 0xFFF ) & CAN_MATCH_FILTER_TYPE ,
noname001 1:235bfa26c5c1 921 CAN_MASK_EID( 0x00) );
noname001 1:235bfa26c5c1 922
noname001 1:235bfa26c5c1 923
noname001 1:235bfa26c5c1 924 CAN1SetTXMode( (char) 0 , CAN_TX_STOP_REQ & CAN_TX_PRIORITY_HIGH );
noname001 1:235bfa26c5c1 925 CAN1SetRXMode( (char) 0 , CAN_RXFUL_CLEAR & CAN_BUF0_DBLBUFFER_DIS );
noname001 1:235bfa26c5c1 926 CAN1SetOperationMode( CAN_IDLE_CON & CAN_CAPTURE_DIS & CAN_MASTERCLOCK_1 &
noname001 1:235bfa26c5c1 927 CAN_REQ_OPERMODE_NOR );
noname001 1:235bfa26c5c1 928 C1INTEbits.RX0IE = 1;
noname001 1:235bfa26c5c1 929 C1INTEbits.RX1IE = 1;
noname001 1:235bfa26c5c1 930 IEC1bits.C1IE = 1;
noname001 1:235bfa26c5c1 931
noname001 1:235bfa26c5c1 932 C1TX0SID = 0x0000;
noname001 1:235bfa26c5c1 933 C1TX0EID = 0x0000;
noname001 1:235bfa26c5c1 934 C1RX0SID = 0x0000;
noname001 1:235bfa26c5c1 935 C1RXF0SID = 0x0010;
noname001 1:235bfa26c5c1 936 }
noname001 1:235bfa26c5c1 937 */
noname001 1:235bfa26c5c1 938 /*
noname001 1:235bfa26c5c1 939 void _ISR _C1Interrupt(void)
noname001 1:235bfa26c5c1 940 {
noname001 1:235bfa26c5c1 941 // 讀取大板子傳來的資料
noname001 1:235bfa26c5c1 942 if (C1RX0CONbits.RXFUL == 1)
noname001 1:235bfa26c5c1 943 {
noname001 1:235bfa26c5c1 944 rotationspeed = C1RX0B1;
noname001 1:235bfa26c5c1 945 omega = (float)rotationspeed * 0.4553 * 0.75; // 2pi * (10/138) (rad/s)
noname001 1:235bfa26c5c1 946 v = 0.33 * omega; // v = r*w (m/s)
noname001 1:235bfa26c5c1 947
noname001 1:235bfa26c5c1 948 C1RX0CONbits.RXFUL = 0;
noname001 1:235bfa26c5c1 949 }
noname001 1:235bfa26c5c1 950
noname001 1:235bfa26c5c1 951 C1INTFbits.RX0IF = 0;
noname001 1:235bfa26c5c1 952
noname001 1:235bfa26c5c1 953 IFS1bits.C1IF = 0;
noname001 1:235bfa26c5c1 954 }
noname001 1:235bfa26c5c1 955 */
noname001 1:235bfa26c5c1 956 /*
noname001 1:235bfa26c5c1 957 void init_ADC(void)
noname001 1:235bfa26c5c1 958 {
noname001 1:235bfa26c5c1 959 ADCON1bits.SSRC = 7; // Internal counter ends sampling and starts conversion (auto convert)
noname001 1:235bfa26c5c1 960 ADCON1bits.ASAM = 1; // Sampling begins immediately after last conversion completes
noname001 1:235bfa26c5c1 961 ADCON2bits.VCFG = 0; // A/D VREFH : AVDD(power) , A/D VREFL : AVSS(ground)
noname001 1:235bfa26c5c1 962 ADCON2bits.CHPS = 0; // 00 = Converts CH0
noname001 1:235bfa26c5c1 963 ADCON3bits.ADRC = 1; // A/D internal RC clock
noname001 1:235bfa26c5c1 964 ADCON3bits.SAMC = 16; // 16*TAD
noname001 1:235bfa26c5c1 965 ADCON3bits.ADCS = 11; // (TCY/2)*(ADCS<5:0>+1)=6*TCY , where ADCS<5:0>=11
noname001 1:235bfa26c5c1 966
noname001 1:235bfa26c5c1 967 ADCHS = 0x0006;; // Connect RB6/AN6 as CH0 input
noname001 1:235bfa26c5c1 968 ADPCFGbits.PCFG6 = 0; // 0 = Analog input pin in Analog mode ; 1: Digital mode
noname001 1:235bfa26c5c1 969
noname001 1:235bfa26c5c1 970 ADCON1bits.ADON = 1;
noname001 1:235bfa26c5c1 971 // 1 = A/D converter module is operating; 0 = A/D converter is off
noname001 1:235bfa26c5c1 972 }
noname001 1:235bfa26c5c1 973 */
noname001 1:235bfa26c5c1 974
noname001 1:235bfa26c5c1 975 float lpf(float input,float output_old,float frequency)
noname001 1:235bfa26c5c1 976 {
noname001 1:235bfa26c5c1 977 float output = 0;
noname001 1:235bfa26c5c1 978
noname001 1:235bfa26c5c1 979 output = (output_old + frequency*T*input) / (1+frequency*T);
noname001 1:235bfa26c5c1 980
noname001 1:235bfa26c5c1 981 return output;
noname001 1:235bfa26c5c1 982 }
noname001 1:235bfa26c5c1 983
noname001 1:235bfa26c5c1 984
noname001 1:235bfa26c5c1 985 float sinr(float input)
noname001 1:235bfa26c5c1 986 {
noname001 1:235bfa26c5c1 987 float output = 0;
noname001 1:235bfa26c5c1 988
noname001 1:235bfa26c5c1 989 output = input - input * input * input / 6;
noname001 1:235bfa26c5c1 990 return output;
noname001 1:235bfa26c5c1 991 }
noname001 1:235bfa26c5c1 992
noname001 1:235bfa26c5c1 993
noname001 1:235bfa26c5c1 994 float cosr(float input)
noname001 1:235bfa26c5c1 995 {
noname001 1:235bfa26c5c1 996 float output = 0;
noname001 1:235bfa26c5c1 997
noname001 1:235bfa26c5c1 998 output = 1 - input * input / 2;
noname001 1:235bfa26c5c1 999 return output;
noname001 1:235bfa26c5c1 1000 }
noname001 1:235bfa26c5c1 1001
noname001 1:235bfa26c5c1 1002
noname001 1:235bfa26c5c1 1003 float arcsin(float input)
noname001 1:235bfa26c5c1 1004 {
noname001 1:235bfa26c5c1 1005 float output = 0;
noname001 1:235bfa26c5c1 1006
noname001 1:235bfa26c5c1 1007 output = input + input * input * input / 6;
noname001 1:235bfa26c5c1 1008 return output;
noname001 1:235bfa26c5c1 1009 }
noname001 1:235bfa26c5c1 1010
noname001 1:235bfa26c5c1 1011
noname001 1:235bfa26c5c1 1012 float arccos(float input)
noname001 1:235bfa26c5c1 1013 {
noname001 1:235bfa26c5c1 1014 float output = 0;
noname001 1:235bfa26c5c1 1015
noname001 1:235bfa26c5c1 1016 output = (3.14 / 2) - arcsin(input);
noname001 1:235bfa26c5c1 1017 return output;
noname001 1:235bfa26c5c1 1018 }
noname001 1:235bfa26c5c1 1019
noname001 1:235bfa26c5c1 1020
noname001 1:235bfa26c5c1 1021 float arctan(float input)
noname001 1:235bfa26c5c1 1022 {
noname001 1:235bfa26c5c1 1023 float output = 0;
noname001 1:235bfa26c5c1 1024
noname001 1:235bfa26c5c1 1025 output = input - input * input * input / 3;
noname001 1:235bfa26c5c1 1026 return output;
noname001 1:235bfa26c5c1 1027 }
noname001 1:235bfa26c5c1 1028
noname001 1:235bfa26c5c1 1029 /*
noname001 1:235bfa26c5c1 1030 void _ISR _U2RXInterrupt(void)
noname001 1:235bfa26c5c1 1031 {
noname001 1:235bfa26c5c1 1032 // 更改體重值
noname001 1:235bfa26c5c1 1033 uart_read = U2RXREG;
noname001 1:235bfa26c5c1 1034
noname001 1:235bfa26c5c1 1035 switch(uart_read)
noname001 1:235bfa26c5c1 1036 {
noname001 1:235bfa26c5c1 1037 case 'q':
noname001 1:235bfa26c5c1 1038 M = 45 + 25; // 人:45kg , 車:25kg
noname001 1:235bfa26c5c1 1039 break;
noname001 1:235bfa26c5c1 1040
noname001 1:235bfa26c5c1 1041 case 'w':
noname001 1:235bfa26c5c1 1042 M = 50 + 25;
noname001 1:235bfa26c5c1 1043 break;
noname001 1:235bfa26c5c1 1044
noname001 1:235bfa26c5c1 1045 case 'e':
noname001 1:235bfa26c5c1 1046 M = 55 + 25;
noname001 1:235bfa26c5c1 1047 break;
noname001 1:235bfa26c5c1 1048
noname001 1:235bfa26c5c1 1049 case 'a':
noname001 1:235bfa26c5c1 1050 M = 60 + 25;
noname001 1:235bfa26c5c1 1051 break;
noname001 1:235bfa26c5c1 1052
noname001 1:235bfa26c5c1 1053 case 's':
noname001 1:235bfa26c5c1 1054 M = 65 + 25;
noname001 1:235bfa26c5c1 1055 break;
noname001 1:235bfa26c5c1 1056
noname001 1:235bfa26c5c1 1057 case 'd':
noname001 1:235bfa26c5c1 1058 M = 70 + 25;
noname001 1:235bfa26c5c1 1059 break;
noname001 1:235bfa26c5c1 1060
noname001 1:235bfa26c5c1 1061 case 'z':
noname001 1:235bfa26c5c1 1062 M = 75 + 25;
noname001 1:235bfa26c5c1 1063 break;
noname001 1:235bfa26c5c1 1064
noname001 1:235bfa26c5c1 1065 case 'x':
noname001 1:235bfa26c5c1 1066 M = 80 + 25;
noname001 1:235bfa26c5c1 1067 break;
noname001 1:235bfa26c5c1 1068
noname001 1:235bfa26c5c1 1069 case 'c':
noname001 1:235bfa26c5c1 1070 M = 0;
noname001 1:235bfa26c5c1 1071 break;
noname001 1:235bfa26c5c1 1072 }
noname001 1:235bfa26c5c1 1073
noname001 1:235bfa26c5c1 1074 IFS1bits.U2RXIF = 0;
noname001 1:235bfa26c5c1 1075 }
noname001 1:235bfa26c5c1 1076 */
noname001 0:0b8d14ed499f 1077 void read_sensor(void)
noname001 0:0b8d14ed499f 1078 {
noname001 0:0b8d14ed499f 1079 // sensor raw data
noname001 0:0b8d14ed499f 1080 Gyro_axis_data[0] = sensor.readRawGyroX();
noname001 0:0b8d14ed499f 1081 Gyro_axis_data[1] = sensor.readRawGyroY();
noname001 0:0b8d14ed499f 1082 Gyro_axis_data[2] = sensor.readRawGyroZ();
noname001 0:0b8d14ed499f 1083
noname001 0:0b8d14ed499f 1084 Acce_axis_data[0] = sensor.readRawAccelX();
noname001 0:0b8d14ed499f 1085 Acce_axis_data[1] = sensor.readRawAccelY();
noname001 0:0b8d14ed499f 1086 Acce_axis_data[2] = sensor.readRawAccelZ();
noname001 0:0b8d14ed499f 1087
noname001 1:235bfa26c5c1 1088 // Magn_axis_data[0] = sensor.readRawMagX();
noname001 1:235bfa26c5c1 1089 // Magn_axis_data[1] = sensor.readRawMagY();
noname001 1:235bfa26c5c1 1090 // Magn_axis_data[2] = sensor.readRawMagZ();
noname001 0:0b8d14ed499f 1091 }