lee sanghun / Mbed 2 deprecated 2020_07_28_10_41

test4

Dependencies:   mbed BufferedSerial LS7366LIB2 FastPWM

encoder.h@0:7cff999a7f5c, 2020-05-11 (annotated)

Committer:
gohgwaja
Date:
Mon May 11 08:47:18 2020 +0000
Revision:
0:7cff999a7f5c
Child:
3:7b195612e26d
hahaha

Who changed what in which revision?

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