数据传输 这部分主要目的是用于调试和显示;这个函数中32位的结果被切割分4个8位数据。 复制代码 1 if (positionX[1]>=0) 2 { //This line compares the sign of the X direction data 3 direction= (direction | 0x10); // if its positive the most significant byte 4 posx_seg[0]= positionX[1] & 0x000000FF; // is set to 1 else it is set to 8 5 posx_seg[1]= (positionX[1]>>8) & 0x000000FF; // the data is also managed in the 6 // subsequent lines in order to be sent. 7 posx_seg[2]= (positionX[1]>>16) & 0x000000FF;// The 32 bit variable must be split into 8 posx_seg[3]= (positionX[1]>>24) & 0x000000FF;// 4 different 8 bit variables in order to 9 // be sent via the 8 bit SCI frame 10 } 11 else 12 {
13 direction=(direction | 0x80); 14 positionXbkp=positionX[1]-1; 15 positionXbkp=positionXbkp^0xFFFFFFFF; 16 posx_seg[0]= positionXbkp & 0x000000FF; 17 posx_seg[1]= (positionXbkp>>8) & 0x000000FF; 18 posx_seg[2]= (positionXbkp>>16) & 0x000000FF; 19 posx_seg[3]= (positionXbkp>>24) & 0x000000FF; 20 } 复制代码
"移动结束"检查 基于积分表示曲线下方区域的概念,速度是加速曲线下方的区域的结果。 如果我们观察一个典型的移动:一个物体沿一个轴从点A移动到点B,一个典型的加速度结果如下图: Ashampoo_Snap_2015.03.28_23h57m09s_008_.png 观察上面的图,加速度先增加后减少直到速度达到最大值(加速度始终为正代表往同一方向加速)。然后以相反的方式加速,直到它再次到达0。在这一点上达到一个稳定的位移和新的位置。 在真实世界中,其中曲线正侧下方的区域面积不等于负侧上方的区域面积,积分结果将永远不会达到零速度,因此将是一个倾斜定位(从未稳定)。 正因为如此,将速度强制减为0非常关键。这是通过不断读取加速度值和0进行比较而实现的。如果在一定数量的采样中,这种情况存在(sample==0)的话,速度将简单地返回为0。 复制代码 1 if (accelerationx[1]==0) // we count the number of acceleration samples that equals zero 2 {
3 countx++; 4 } 5 else 6 {
7 countx =0; 8 } 9 if (countx>=25) // if this number exceeds 25, we can assume that velocity is zero 10 {
11 velocityx[1]=0; 12 velocityx[0]=0; 13 } 复制代码
Ashampoo_Snap_2015.03.29_00h24m36s_010_.png
源代码
复制代码 1 #include <hidef.h> 2 #include “derivative.h” 3 #include “adc.h” 4 #include “buzzer.h” 5 #include “SCItx.h” 6 #pragma DATA_SEG MY_ZEROPAGE 7 unsigned char near Sample_X; 8 unsigned char near Sample_Y; 9 unsigned char near Sample_Z; 10 unsigned char near Sensor_Data[8]; 11 unsigned char near countx,county ; 12 signed int near accelerationx[2], accelerationy[2]; 13 signed long near velocityx[2], velocityy[2]; 14 signed long near positionX[2]; 15 signed long near positionY[2]; 16 signed long near positionZ[2]; 17 unsigned char near direction; 18 unsigned long near sstatex,sstatey; 19 #pragma DATA_SEG DEFAULT 20 void init(void); 21 void Calibrate(void); 22 void data_transfer(void); 23 void concatenate_data(void); 24 void movement_end_check(void); 25 void position(void); 26 void main (void) 27 {
28 init(); 29 get_threshold(); 30 do 31 {
32 position(); 33 }while(1); 34 } 35 /******************************************************************************* 36 The purpose of the calibration routine is to obtain the value of the reference threshold. 37 It consists on a 1024 samples average in no-movement condition. 38 / 53 /********************************************************************************* 54 This function obtains magnitude and direction 55 In this particular protocol direction and magnitude are sent in separate variables. 56 Management can be done in many other different ways. 57 / 58 void data_transfer(void) 59 {
60 signed long positionXbkp; 61 signed long positionYbkp; 62 unsigned int delay; 63 unsigned char posx_seg[4], posy_seg[4]; 64 if (positionX[1]>=0) { //This line compares the sign of the X direction data 65 direction= (direction | 0x10); //if its positive the most significant byte 66 posx_seg[0]= positionX[1] & 0x000000FF; // is set to 1 else it is set to 8 67 posx_seg[1]= (positionX[1]>>8) & 0x000000FF; // the data is also managed in the 68 // subsequent lines in order to 69 posx_seg[2]= (positionX[1]>>16) & 0x000000FF; // be sent. The 32 bit variable must be 70 posx_seg[3]= (positionX[1]>>24) & 0x000000FF; // split into 4 different 8 bit 71 // variables in order to be sent via 72 // the 8 bit SCI frame 73 } 74 else {direction=(direction | 0x80); 75 positionXbkp=positionX[1]-1; 76 positionXbkp=positionXbkp^0xFFFFFFFF; 77 posx_seg[0]= positionXbkp & 0x000000FF; 78 posx_seg[1]= (positionXbkp>>8) & 0x000000FF; 79 posx_seg[2]= (positionXbkp>>16) & 0x000000FF; 80 posx_seg[3]= (positionXbkp>>24) & 0x000000FF; 81 } 82 if (positionY[1]>=0) { // Same management than in the previous case 83 direction= (direction | 0x08); // but with the Y data. 84 posy_seg[0]= positionY[1] & 0x000000FF; 85 posy_seg[1]= (positionY[1]>>8) & 0x000000FF; 86 posy_seg[2]= (positionY[1]>>16) & 0x000000FF; 87 posy_seg[3]= (positionY[1]>>24) & 0x000000FF; 88 } 89 else {direction= (direction | 0x01); 90 positionYbkp=positionY[1]-1; 91 positionYbkp=positionYbkp^0xFFFFFFFF; 92 posy_seg[0]= positionYbkp & 0x000000FF; 93 posy_seg[1]= (positionYbkp>>8) & 0x000000FF; 94 posy_seg[2]= (positionYbkp>>16) & 0x000000FF; 95 posy_seg[3]= (positionYbkp>>24) & 0x000000FF; 96 } 97 delay = 0x0100; 98 Sensor_Data[0] = 0x03; 99 Sensor_Data[1] = direction; 100 Sensor_Data[2] = posx_seg[3]; 101 Sensor_Data[3] = posy_seg[3]; 102 Sensor_Data[4] = 0x01; 103 Sensor_Data[5] = 0x01; 104 Sensor_Data[6] = END_OF_FRAME; 105 while (–delay); 106 SCITxMsg(Sensor_Data); // Data transferring function 107 while (SCIC2 & 0x08); 108 } 109 // 110 /****************************************************************************************** 111 This function returns data format to its original state. When obtaining the magnitude and 112 direction of the position, an inverse two’s complement is made. This function makes the two’s 113 complement in order to return the data to it original state. 114 It is important to notice that the sensibility adjustment is greatly impacted here, the amount 115 of “ones” inserted in the mask must be equivalent to the “ones” lost in the shifting made in 116 the previous function upon the sensibility modification. 117 / 118 void data_reintegration(void) 119 {
120 if (direction >=10) 121 {positionX[1]= positionX[1]|0xFFFFC000;} // 18 “ones” inserted. Same size as the 122 //amount of shifts 123 direction = direction & 0x01; 124 if (direction ==1) 125 {positionY[1]= positionY[1]|0xFFFFC000;} 126 } 127 / 128 This function allows movement end detection. If a certain number of acceleration samples are 129 equal to zero we can assume movement has stopped. Accumulated Error generated in the velocity 130 calculations is eliminated by resetting the velocity variables. This stops position increment 131 and greatly eliminates position error. 132 / 133 void movement_end_check(void) 134 {
135 if (accelerationx[1]==0) //we count the number of acceleration samples that equals cero 136 { countx++;} 137 else { countx =0;} 138 if (countx>=25) //if this number exceeds 25, we can assume that velocity is cero 139 {
140 velocityx[1]=0; 141 velocityx[0]=0; 142 } 143 if (accelerationy[1]==0) //we do the same for the Y axis 144 { county++;} 145 else { county =0;} 146 if (county>=25) 147 {
148 velocityy[1]=0; 149 velocityy[0]=0; 150 } 151 } 152 // 153 /***************************************************************************************** 154 This function transforms acceleration to a proportional position by integrating the 155 acceleration data twice. It also adjusts sensibility by multiplying the “positionX” and 156 “positionY” variables. 157 This integration algorithm carries error, which is compensated in the “movenemt_end_check” 158 subroutine. Faster sampling frequency implies less error but requires more memory. Keep in 159 mind that the same process is applied to the X and Y axis. 160 / 161 void position(void) 162 {
163 unsigned char count2 ; 164 count2=0; 165 do{
166 ADC_GetAllAxis(); 167 accelerationx[1]=accelerationx[1] + Sample_X; //filtering routine for noise attenuation 168 accelerationy[1]=accelerationy[1] + Sample_Y; //64 samples are averaged. The resulting 169 //average represents the acceleration of 170 //an instant 171 count2++; 172 }while (count2!=0x40); // 64 sums of the acceleration sample 173 accelerationx[1]= accelerationx[1]>>6; // division by 64 174 accelerationy[1]= accelerationy[1]>>6; 175 accelerationx[1] = accelerationx[1] - (int)sstatex; //eliminating zero reference 176 //offset of the acceleration data 177 accelerationy[1] = accelerationy[1] - (int)sstatey; // to obtain positive and negative 178 //acceleration 179 if ((accelerationx[1] <=3)&&(accelerationx[1] >= -3)) //Discrimination window applied 180 {accelerationx[1] = 0;} // to the X axis acceleration 181 //variable 182 if ((accelerationy[1] <=3)&&(accelerationy[1] >= -3)) 183 {accelerationy[1] = 0;} 184 //first X integration: 185 velocityx[1]= velocityx[0]+ accelerationx[0]+ ((accelerationx[1] -accelerationx[0])>>1); 186 //second X integration: 187 positionX[1]= positionX[0] + velocityx[0] + ((velocityx[1] - velocityx[0])>>1); 188 //first Y integration: 189 velocityy[1] = velocityy[0] + accelerationy[0] + ((accelerationy[1] -accelerationy[0])>>1); 190 //second Y integration: 191 positionY[1] = positionY[0] + velocityy[0] + ((velocityy[1] - velocityy[0])>>1); 192 accelerationx[0] = accelerationx[1]; //The current acceleration value must be sent 193 //to the previous acceleration 194 accelerationy[0] = accelerationy[1]; //variable in order to introduce the new 195 //acceleration value. 196 velocityx[0] = velocityx[1]; //Same done for the velocity variable 197 velocityy[0] = velocityy[1]; 198 positionX[1] = positionX[1]<<18; //The idea behind this shifting (multiplication) 199 //is a sensibility adjustment. 200 positionY[1] = positionY[1]<<18; //Some applications require adjustments to a 201 //particular situation 202 //i.e. mouse application 203 data_transfer(); 204 positionX[1] = positionX[1]>>18; //once the variables are sent them must return to 205 positionY[1] = positionY[1]>>18; //their original state 206 movement_end_check(); 207 positionX[0] = positionX[1]; //actual position data must be sent to the 208 positionY[0] = positionY[1]; //previous position 209 direction = 0; // data variable to direction variable reset 210 } 211 // 复制代码