lee sanghun / Mbed 2 deprecated 2020_04_21_11_20

test

Dependencies:   mbed BufferedSerial LS7366LIB2 FastPWM

encoder.h@0:e12eb40b9fef, 2020-04-23 (annotated)

Committer:
lsh2205
Date:
Thu Apr 23 00:38:16 2020 +0000
Revision:
0:e12eb40b9fef
test;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lsh2205 0:e12eb40b9fef 1 #ifndef _ENCODER_H_
lsh2205 0:e12eb40b9fef 2 #define _ENCODER_H_
lsh2205 0:e12eb40b9fef 3
lsh2205 0:e12eb40b9fef 4 int ex_encoder_data[6]={0,};
lsh2205 0:e12eb40b9fef 5 int dif_encoder_data[6]={0,};
lsh2205 0:e12eb40b9fef 6 double filter_dif_encoder_data[6]={0,};
lsh2205 0:e12eb40b9fef 7 double filter_dif_encoder_co[6] = {0.01,0.01,0.01,0.01,0.01,0.01};
lsh2205 0:e12eb40b9fef 8
lsh2205 0:e12eb40b9fef 9 void filter_encoder_data()
lsh2205 0:e12eb40b9fef 10 {
lsh2205 0:e12eb40b9fef 11 for(int i=0; i<6;i++)
lsh2205 0:e12eb40b9fef 12 {
lsh2205 0:e12eb40b9fef 13 dif_encoder_data[i]= encoder_data[i] - ex_encoder_data[i];
lsh2205 0:e12eb40b9fef 14
lsh2205 0:e12eb40b9fef 15 filter_dif_encoder_data[i] = filter_dif_encoder_data[i]*(1-filter_dif_encoder_co[i]) + (double)dif_encoder_data[i]*filter_dif_encoder_co[i];
lsh2205 0:e12eb40b9fef 16
lsh2205 0:e12eb40b9fef 17 ex_encoder_data[i] = encoder_data[i];
lsh2205 0:e12eb40b9fef 18 }
lsh2205 0:e12eb40b9fef 19 }
lsh2205 0:e12eb40b9fef 20
lsh2205 0:e12eb40b9fef 21 void encoder_read_raw()
lsh2205 0:e12eb40b9fef 22 {
lsh2205 0:e12eb40b9fef 23 encoder_data[0] = encoder1.read();
lsh2205 0:e12eb40b9fef 24 encoder_data[1] = encoder2.read();
lsh2205 0:e12eb40b9fef 25 encoder_data[2] = encoder3.read();
lsh2205 0:e12eb40b9fef 26 encoder_data[3] = encoder4.read();
lsh2205 0:e12eb40b9fef 27 encoder_data[4] = -encoder5.read();
lsh2205 0:e12eb40b9fef 28 encoder_data[5] = -encoder6.read();
lsh2205 0:e12eb40b9fef 29
lsh2205 0:e12eb40b9fef 30 }
lsh2205 0:e12eb40b9fef 31
lsh2205 0:e12eb40b9fef 32
lsh2205 0:e12eb40b9fef 33 void encoder_read()
lsh2205 0:e12eb40b9fef 34 {
lsh2205 0:e12eb40b9fef 35 encoder_data[0] = encoder1.read();
lsh2205 0:e12eb40b9fef 36 encoder_data[1] = encoder2.read();
lsh2205 0:e12eb40b9fef 37 encoder_data[2] = encoder3.read();
lsh2205 0:e12eb40b9fef 38 encoder_data[3] = encoder4.read();
lsh2205 0:e12eb40b9fef 39 encoder_data[4] = -encoder5.read();
lsh2205 0:e12eb40b9fef 40 encoder_data[5] = -encoder6.read();
lsh2205 0:e12eb40b9fef 41
lsh2205 0:e12eb40b9fef 42 /*
lsh2205 0:e12eb40b9fef 43 for(int i=0;i<6;i++)
lsh2205 0:e12eb40b9fef 44 {
lsh2205 0:e12eb40b9fef 45 pc.printf("%8d", encoder_data[i]);
lsh2205 0:e12eb40b9fef 46 }pc.printf("\r\n");
lsh2205 0:e12eb40b9fef 47 */
lsh2205 0:e12eb40b9fef 48 filter_encoder_data();
lsh2205 0:e12eb40b9fef 49 }
lsh2205 0:e12eb40b9fef 50
lsh2205 0:e12eb40b9fef 51 /*
lsh2205 0:e12eb40b9fef 52 void reset_check()
lsh2205 0:e12eb40b9fef 53 {
lsh2205 0:e12eb40b9fef 54 }
lsh2205 0:e12eb40b9fef 55 */
lsh2205 0:e12eb40b9fef 56 void reset_check()
lsh2205 0:e12eb40b9fef 57 {
lsh2205 0:e12eb40b9fef 58 if(encoder_data[0]==-1 && encoder_data[1]==-1 && encoder_data[2]==-1 && encoder_data[3]==-1 && encoder_data[4]==1 && encoder_data[5]==1)
lsh2205 0:e12eb40b9fef 59 {
lsh2205 0:e12eb40b9fef 60 motor_onoff[0]=false;
lsh2205 0:e12eb40b9fef 61 motor_onoff[1]=false;
lsh2205 0:e12eb40b9fef 62 motor_onoff[2]=false;
lsh2205 0:e12eb40b9fef 63 motor_onoff[3]=false;
lsh2205 0:e12eb40b9fef 64 motor_onoff[4]=false;
lsh2205 0:e12eb40b9fef 65 motor_onoff[5]=false;
lsh2205 0:e12eb40b9fef 66
lsh2205 0:e12eb40b9fef 67 while(1)
lsh2205 0:e12eb40b9fef 68 {
lsh2205 0:e12eb40b9fef 69
lsh2205 0:e12eb40b9fef 70 pwm1.write(0.4);
lsh2205 0:e12eb40b9fef 71 pwm2.write(0.4);
lsh2205 0:e12eb40b9fef 72 pwm3.write(0.4);
lsh2205 0:e12eb40b9fef 73 pwm4.write(0.4);
lsh2205 0:e12eb40b9fef 74 pwm5.write(0.4);
lsh2205 0:e12eb40b9fef 75 pwm6.write(0.4);
lsh2205 0:e12eb40b9fef 76
lsh2205 0:e12eb40b9fef 77 wait_ms(50);
lsh2205 0:e12eb40b9fef 78
lsh2205 0:e12eb40b9fef 79 encoder_data[0] = encoder1.read();
lsh2205 0:e12eb40b9fef 80 if(encoder_data[0]!=-1)
lsh2205 0:e12eb40b9fef 81 {
lsh2205 0:e12eb40b9fef 82 NVIC_SystemReset();
lsh2205 0:e12eb40b9fef 83 }
lsh2205 0:e12eb40b9fef 84 wait_ms(200);
lsh2205 0:e12eb40b9fef 85 }
lsh2205 0:e12eb40b9fef 86
lsh2205 0:e12eb40b9fef 87 }
lsh2205 0:e12eb40b9fef 88 }
lsh2205 0:e12eb40b9fef 89
lsh2205 0:e12eb40b9fef 90 void encoder_reset_cnt()
lsh2205 0:e12eb40b9fef 91 {
lsh2205 0:e12eb40b9fef 92 encoder1.LS7366_reset_counter();
lsh2205 0:e12eb40b9fef 93 encoder2.LS7366_reset_counter();
lsh2205 0:e12eb40b9fef 94 encoder3.LS7366_reset_counter();
lsh2205 0:e12eb40b9fef 95 encoder4.LS7366_reset_counter();
lsh2205 0:e12eb40b9fef 96 encoder5.LS7366_reset_counter();
lsh2205 0:e12eb40b9fef 97 encoder6.LS7366_reset_counter();
lsh2205 0:e12eb40b9fef 98 }
lsh2205 0:e12eb40b9fef 99
lsh2205 0:e12eb40b9fef 100
lsh2205 0:e12eb40b9fef 101 void Time_To_Motor_Drive(int dir, double time_ms)
lsh2205 0:e12eb40b9fef 102 {
lsh2205 0:e12eb40b9fef 103 const int motor_percent[6] = {400, 400, 400, 350, -200, -200};
lsh2205 0:e12eb40b9fef 104 int motor_4_dir = 1;
lsh2205 0:e12eb40b9fef 105
lsh2205 0:e12eb40b9fef 106 for(int i = 0; i < 6; i++)
lsh2205 0:e12eb40b9fef 107 {
lsh2205 0:e12eb40b9fef 108 if(i == 3)
lsh2205 0:e12eb40b9fef 109 {
lsh2205 0:e12eb40b9fef 110 motor_4_dir = -1;
lsh2205 0:e12eb40b9fef 111 }
lsh2205 0:e12eb40b9fef 112 else
lsh2205 0:e12eb40b9fef 113 {
lsh2205 0:e12eb40b9fef 114 motor_4_dir = 1;
lsh2205 0:e12eb40b9fef 115 }
lsh2205 0:e12eb40b9fef 116 motor_power(i, motor_percent[i] * dir * motor_4_dir);
lsh2205 0:e12eb40b9fef 117 }
lsh2205 0:e12eb40b9fef 118 wait_ms(time_ms);
lsh2205 0:e12eb40b9fef 119
lsh2205 0:e12eb40b9fef 120 if(dir == -1)
lsh2205 0:e12eb40b9fef 121 {
lsh2205 0:e12eb40b9fef 122 for(int i = 4; i < 6; i++)
lsh2205 0:e12eb40b9fef 123 {
lsh2205 0:e12eb40b9fef 124 motor_power(i, 0);
lsh2205 0:e12eb40b9fef 125 }
lsh2205 0:e12eb40b9fef 126 wait_ms(time_ms);
lsh2205 0:e12eb40b9fef 127 }
lsh2205 0:e12eb40b9fef 128 }
lsh2205 0:e12eb40b9fef 129
lsh2205 0:e12eb40b9fef 130
lsh2205 0:e12eb40b9fef 131 void encoder_check0()
lsh2205 0:e12eb40b9fef 132 {
lsh2205 0:e12eb40b9fef 133
lsh2205 0:e12eb40b9fef 134 int forward_enc[6] = {0,};
lsh2205 0:e12eb40b9fef 135 int reverse_enc[6] = {0,};
lsh2205 0:e12eb40b9fef 136 bool encoder_state = false;
lsh2205 0:e12eb40b9fef 137
lsh2205 0:e12eb40b9fef 138 Time_To_Motor_Drive(0, 200);
lsh2205 0:e12eb40b9fef 139 encoder_reset_cnt();
lsh2205 0:e12eb40b9fef 140 encoder_read();
lsh2205 0:e12eb40b9fef 141
lsh2205 0:e12eb40b9fef 142 for(int i = 0; i < 4; i++)
lsh2205 0:e12eb40b9fef 143 {
lsh2205 0:e12eb40b9fef 144 if(encoder_data[i] == -1)
lsh2205 0:e12eb40b9fef 145 {
lsh2205 0:e12eb40b9fef 146 encoder_state = true;
lsh2205 0:e12eb40b9fef 147 pc.printf("%d : -1 \n\r", i);
lsh2205 0:e12eb40b9fef 148 }
lsh2205 0:e12eb40b9fef 149 }
lsh2205 0:e12eb40b9fef 150 for(int i = 4; i < 6; i++)
lsh2205 0:e12eb40b9fef 151 {
lsh2205 0:e12eb40b9fef 152 if(encoder_data[i] == 1)
lsh2205 0:e12eb40b9fef 153 {
lsh2205 0:e12eb40b9fef 154 encoder_state = true;
lsh2205 0:e12eb40b9fef 155 pc.printf("%d : -1 \n\r", i);
lsh2205 0:e12eb40b9fef 156 }
lsh2205 0:e12eb40b9fef 157 }
lsh2205 0:e12eb40b9fef 158
lsh2205 0:e12eb40b9fef 159 if(encoder_state != true)
lsh2205 0:e12eb40b9fef 160 {
lsh2205 0:e12eb40b9fef 161 Time_To_Motor_Drive(1, 200);
lsh2205 0:e12eb40b9fef 162 Time_To_Motor_Drive(0, 1000);
lsh2205 0:e12eb40b9fef 163 encoder_read();
lsh2205 0:e12eb40b9fef 164
lsh2205 0:e12eb40b9fef 165 for(int i = 0; i < 6; i++)
lsh2205 0:e12eb40b9fef 166 {
lsh2205 0:e12eb40b9fef 167 forward_enc[i] = encoder_data[i];// * -1;
lsh2205 0:e12eb40b9fef 168 }
lsh2205 0:e12eb40b9fef 169 Time_To_Motor_Drive(-1, 250);
lsh2205 0:e12eb40b9fef 170 Time_To_Motor_Drive(0, 1000);
lsh2205 0:e12eb40b9fef 171 encoder_read();
lsh2205 0:e12eb40b9fef 172
lsh2205 0:e12eb40b9fef 173 for(int i = 0; i < 6; i++)
lsh2205 0:e12eb40b9fef 174 {
lsh2205 0:e12eb40b9fef 175 reverse_enc[i] = encoder_data[i];// * -1;
lsh2205 0:e12eb40b9fef 176 if((forward_enc[i] > 100) || (forward_enc[i] > reverse_enc[i]) || (forward_enc[i] == 0 && reverse_enc[i] == 0))
lsh2205 0:e12eb40b9fef 177 {
lsh2205 0:e12eb40b9fef 178 encoder_state = true;
lsh2205 0:e12eb40b9fef 179 pc.printf("%d ", i);
lsh2205 0:e12eb40b9fef 180 }
lsh2205 0:e12eb40b9fef 181 }
lsh2205 0:e12eb40b9fef 182 }
lsh2205 0:e12eb40b9fef 183
lsh2205 0:e12eb40b9fef 184 //error
lsh2205 0:e12eb40b9fef 185 if(encoder_state == true)
lsh2205 0:e12eb40b9fef 186 {
lsh2205 0:e12eb40b9fef 187 pc.printf("Encoder error!\n\r");
lsh2205 0:e12eb40b9fef 188 wait_ms(1000);
lsh2205 0:e12eb40b9fef 189 while(1)
lsh2205 0:e12eb40b9fef 190 {
lsh2205 0:e12eb40b9fef 191 }
lsh2205 0:e12eb40b9fef 192 }
lsh2205 0:e12eb40b9fef 193 }
lsh2205 0:e12eb40b9fef 194
lsh2205 0:e12eb40b9fef 195 void encoder_check2()
lsh2205 0:e12eb40b9fef 196 {
lsh2205 0:e12eb40b9fef 197 encoder_reset_cnt();
lsh2205 0:e12eb40b9fef 198
lsh2205 0:e12eb40b9fef 199
lsh2205 0:e12eb40b9fef 200 encoder_read();
lsh2205 0:e12eb40b9fef 201
lsh2205 0:e12eb40b9fef 202 for(int i=0;i<6;i++)
lsh2205 0:e12eb40b9fef 203 {
lsh2205 0:e12eb40b9fef 204 pc.printf("%8d", encoder_data[i]);
lsh2205 0:e12eb40b9fef 205 }pc.printf("\r\n");
lsh2205 0:e12eb40b9fef 206
lsh2205 0:e12eb40b9fef 207 int encoder_check_move[6]={0,0,0,0,0,0};
lsh2205 0:e12eb40b9fef 208
lsh2205 0:e12eb40b9fef 209 for(int i=0; i<200;i++)
lsh2205 0:e12eb40b9fef 210 {
lsh2205 0:e12eb40b9fef 211 encoder_read();
lsh2205 0:e12eb40b9fef 212
lsh2205 0:e12eb40b9fef 213 for(int k=0;k<6;k++)
lsh2205 0:e12eb40b9fef 214 {
lsh2205 0:e12eb40b9fef 215 if(encoder_check[k]==false)
lsh2205 0:e12eb40b9fef 216 {
lsh2205 0:e12eb40b9fef 217 if(encoder_data[k]<-3)
lsh2205 0:e12eb40b9fef 218 encoder_check[k]=true;
lsh2205 0:e12eb40b9fef 219 if(encoder_data[k]>3)
lsh2205 0:e12eb40b9fef 220 encoder_check[k]=true;
lsh2205 0:e12eb40b9fef 221 encoder_check_move[k]=i;
lsh2205 0:e12eb40b9fef 222 }
lsh2205 0:e12eb40b9fef 223
lsh2205 0:e12eb40b9fef 224
lsh2205 0:e12eb40b9fef 225 if(encoder_check[k])
lsh2205 0:e12eb40b9fef 226 {
lsh2205 0:e12eb40b9fef 227 motor_power(k, 0);
lsh2205 0:e12eb40b9fef 228 }
lsh2205 0:e12eb40b9fef 229 else
lsh2205 0:e12eb40b9fef 230 {
lsh2205 0:e12eb40b9fef 231 motor_power(k, i);
lsh2205 0:e12eb40b9fef 232 }
lsh2205 0:e12eb40b9fef 233 }
lsh2205 0:e12eb40b9fef 234 }
lsh2205 0:e12eb40b9fef 235
lsh2205 0:e12eb40b9fef 236
lsh2205 0:e12eb40b9fef 237 for(int i=0;i<6;i++)
lsh2205 0:e12eb40b9fef 238 motor_power(i, 0);
lsh2205 0:e12eb40b9fef 239
lsh2205 0:e12eb40b9fef 240 wait_ms(100);
lsh2205 0:e12eb40b9fef 241
lsh2205 0:e12eb40b9fef 242 for(int i=0;i<6;i++)
lsh2205 0:e12eb40b9fef 243 {
lsh2205 0:e12eb40b9fef 244 pc.printf("%8d", encoder_data[i]);
lsh2205 0:e12eb40b9fef 245 }pc.printf("\r\n");
lsh2205 0:e12eb40b9fef 246
lsh2205 0:e12eb40b9fef 247 for(int i=0; i<200;i++)
lsh2205 0:e12eb40b9fef 248 {
lsh2205 0:e12eb40b9fef 249 encoder_read();
lsh2205 0:e12eb40b9fef 250 for(int k=0;k<6;k++)
lsh2205 0:e12eb40b9fef 251 {
lsh2205 0:e12eb40b9fef 252 if(encoder_check[k]==false)
lsh2205 0:e12eb40b9fef 253 motor_power(k, -i);
lsh2205 0:e12eb40b9fef 254 }
lsh2205 0:e12eb40b9fef 255 }
lsh2205 0:e12eb40b9fef 256
lsh2205 0:e12eb40b9fef 257 for(int i=0;i<6;i++)
lsh2205 0:e12eb40b9fef 258 motor_power(i, 0);
lsh2205 0:e12eb40b9fef 259
lsh2205 0:e12eb40b9fef 260
lsh2205 0:e12eb40b9fef 261 wait_ms(100);
lsh2205 0:e12eb40b9fef 262
lsh2205 0:e12eb40b9fef 263 encoder_read();
lsh2205 0:e12eb40b9fef 264 for(int i=0;i<6;i++)
lsh2205 0:e12eb40b9fef 265 {
lsh2205 0:e12eb40b9fef 266 pc.printf("%8d", encoder_data[i]);
lsh2205 0:e12eb40b9fef 267 }pc.printf("\r\n");
lsh2205 0:e12eb40b9fef 268 for(int i=0;i<6;i++)
lsh2205 0:e12eb40b9fef 269 {
lsh2205 0:e12eb40b9fef 270 pc.printf("%8d", encoder_check_move[i]);
lsh2205 0:e12eb40b9fef 271 }pc.printf("\r\n");
lsh2205 0:e12eb40b9fef 272
lsh2205 0:e12eb40b9fef 273 for(int i=0;i<6;i++)
lsh2205 0:e12eb40b9fef 274 {
lsh2205 0:e12eb40b9fef 275 if(encoder_check[i])
lsh2205 0:e12eb40b9fef 276 pc.printf(" OK");
lsh2205 0:e12eb40b9fef 277 else
lsh2205 0:e12eb40b9fef 278 pc.printf(" ER");
lsh2205 0:e12eb40b9fef 279
lsh2205 0:e12eb40b9fef 280 motor_onoff[i]=encoder_check[i];
lsh2205 0:e12eb40b9fef 281 }pc.printf("\r\n");
lsh2205 0:e12eb40b9fef 282
lsh2205 0:e12eb40b9fef 283
lsh2205 0:e12eb40b9fef 284
lsh2205 0:e12eb40b9fef 285 }
lsh2205 0:e12eb40b9fef 286
lsh2205 0:e12eb40b9fef 287 #endif