lee sanghun / Mbed 2 deprecated 2020_07_28_10_41

test4

Dependencies:   mbed BufferedSerial LS7366LIB2 FastPWM

encoder.h@4:bf278ddb8504, 2020-12-08 (annotated)

Committer:
lsh3146
Date:
Tue Dec 08 01:25:06 2020 +0000
Revision:
4:bf278ddb8504
Parent:
0:7cff999a7f5c 
aaaaaqqqqq

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 {
lsh3146 4:bf278ddb8504 58 }
gohgwaja 0:7cff999a7f5c 59
lsh3146 4:bf278ddb8504 60 /*
gohgwaja 0:7cff999a7f5c 61 encoder_data[0] = encoder1.read();
gohgwaja 0:7cff999a7f5c 62 if(encoder_data[0]!=-1)
gohgwaja 0:7cff999a7f5c 63 {
gohgwaja 0:7cff999a7f5c 64 NVIC_SystemReset();
gohgwaja 0:7cff999a7f5c 65 }
lsh3146 4:bf278ddb8504 66 */
gohgwaja 0:7cff999a7f5c 67
gohgwaja 0:7cff999a7f5c 68
gohgwaja 0:7cff999a7f5c 69 void encoder_reset_cnt()
gohgwaja 0:7cff999a7f5c 70 {
gohgwaja 0:7cff999a7f5c 71 encoder1.LS7366_reset_counter();
gohgwaja 0:7cff999a7f5c 72 encoder2.LS7366_reset_counter();
gohgwaja 0:7cff999a7f5c 73 encoder3.LS7366_reset_counter();
gohgwaja 0:7cff999a7f5c 74 encoder4.LS7366_reset_counter();
gohgwaja 0:7cff999a7f5c 75 encoder5.LS7366_reset_counter();
gohgwaja 0:7cff999a7f5c 76 encoder6.LS7366_reset_counter();
gohgwaja 0:7cff999a7f5c 77 }
gohgwaja 0:7cff999a7f5c 78
gohgwaja 0:7cff999a7f5c 79
gohgwaja 0:7cff999a7f5c 80 void Time_To_Motor_Drive(int dir, double time_ms)
gohgwaja 0:7cff999a7f5c 81 {
gohgwaja 0:7cff999a7f5c 82 const int motor_percent[6] = {400, 400, 400, 350, -200, -200};
gohgwaja 0:7cff999a7f5c 83 int motor_4_dir = 1;
gohgwaja 0:7cff999a7f5c 84
gohgwaja 0:7cff999a7f5c 85 for(int i = 0; i < 6; i++)
gohgwaja 0:7cff999a7f5c 86 {
gohgwaja 0:7cff999a7f5c 87 if(i == 3)
gohgwaja 0:7cff999a7f5c 88 {
gohgwaja 0:7cff999a7f5c 89 motor_4_dir = -1;
gohgwaja 0:7cff999a7f5c 90 }
gohgwaja 0:7cff999a7f5c 91 else
gohgwaja 0:7cff999a7f5c 92 {
gohgwaja 0:7cff999a7f5c 93 motor_4_dir = 1;
gohgwaja 0:7cff999a7f5c 94 }
gohgwaja 0:7cff999a7f5c 95 motor_power(i, motor_percent[i] * dir * motor_4_dir);
gohgwaja 0:7cff999a7f5c 96 }
gohgwaja 0:7cff999a7f5c 97 wait_ms(time_ms);
gohgwaja 0:7cff999a7f5c 98
gohgwaja 0:7cff999a7f5c 99 if(dir == -1)
gohgwaja 0:7cff999a7f5c 100 {
gohgwaja 0:7cff999a7f5c 101 for(int i = 4; i < 6; i++)
gohgwaja 0:7cff999a7f5c 102 {
gohgwaja 0:7cff999a7f5c 103 motor_power(i, 0);
gohgwaja 0:7cff999a7f5c 104 }
gohgwaja 0:7cff999a7f5c 105 wait_ms(time_ms);
gohgwaja 0:7cff999a7f5c 106 }
gohgwaja 0:7cff999a7f5c 107 }
gohgwaja 0:7cff999a7f5c 108
gohgwaja 0:7cff999a7f5c 109
gohgwaja 0:7cff999a7f5c 110 void encoder_check0()
gohgwaja 0:7cff999a7f5c 111 {
gohgwaja 0:7cff999a7f5c 112
gohgwaja 0:7cff999a7f5c 113 int forward_enc[6] = {0,};
gohgwaja 0:7cff999a7f5c 114 int reverse_enc[6] = {0,};
gohgwaja 0:7cff999a7f5c 115 bool encoder_state = false;
gohgwaja 0:7cff999a7f5c 116
gohgwaja 0:7cff999a7f5c 117 Time_To_Motor_Drive(0, 200);
gohgwaja 0:7cff999a7f5c 118 encoder_reset_cnt();
gohgwaja 0:7cff999a7f5c 119 encoder_read();
gohgwaja 0:7cff999a7f5c 120
gohgwaja 0:7cff999a7f5c 121 for(int i = 0; i < 4; i++)
gohgwaja 0:7cff999a7f5c 122 {
gohgwaja 0:7cff999a7f5c 123 if(encoder_data[i] == -1)
gohgwaja 0:7cff999a7f5c 124 {
gohgwaja 0:7cff999a7f5c 125 encoder_state = true;
gohgwaja 0:7cff999a7f5c 126 pc.printf("%d : -1 \n\r", i);
gohgwaja 0:7cff999a7f5c 127 }
gohgwaja 0:7cff999a7f5c 128 }
gohgwaja 0:7cff999a7f5c 129 for(int i = 4; i < 6; i++)
gohgwaja 0:7cff999a7f5c 130 {
gohgwaja 0:7cff999a7f5c 131 if(encoder_data[i] == 1)
gohgwaja 0:7cff999a7f5c 132 {
gohgwaja 0:7cff999a7f5c 133 encoder_state = true;
gohgwaja 0:7cff999a7f5c 134 pc.printf("%d : -1 \n\r", i);
gohgwaja 0:7cff999a7f5c 135 }
gohgwaja 0:7cff999a7f5c 136 }
gohgwaja 0:7cff999a7f5c 137
gohgwaja 0:7cff999a7f5c 138 if(encoder_state != true)
gohgwaja 0:7cff999a7f5c 139 {
gohgwaja 0:7cff999a7f5c 140 Time_To_Motor_Drive(1, 200);
gohgwaja 0:7cff999a7f5c 141 Time_To_Motor_Drive(0, 1000);
gohgwaja 0:7cff999a7f5c 142 encoder_read();
gohgwaja 0:7cff999a7f5c 143
gohgwaja 0:7cff999a7f5c 144 for(int i = 0; i < 6; i++)
gohgwaja 0:7cff999a7f5c 145 {
gohgwaja 0:7cff999a7f5c 146 forward_enc[i] = encoder_data[i];// * -1;
gohgwaja 0:7cff999a7f5c 147 }
gohgwaja 0:7cff999a7f5c 148 Time_To_Motor_Drive(-1, 250);
gohgwaja 0:7cff999a7f5c 149 Time_To_Motor_Drive(0, 1000);
gohgwaja 0:7cff999a7f5c 150 encoder_read();
gohgwaja 0:7cff999a7f5c 151
gohgwaja 0:7cff999a7f5c 152 for(int i = 0; i < 6; i++)
gohgwaja 0:7cff999a7f5c 153 {
gohgwaja 0:7cff999a7f5c 154 reverse_enc[i] = encoder_data[i];// * -1;
gohgwaja 0:7cff999a7f5c 155 if((forward_enc[i] > 100) || (forward_enc[i] > reverse_enc[i]) || (forward_enc[i] == 0 && reverse_enc[i] == 0))
gohgwaja 0:7cff999a7f5c 156 {
gohgwaja 0:7cff999a7f5c 157 encoder_state = true;
gohgwaja 0:7cff999a7f5c 158 pc.printf("%d ", i);
gohgwaja 0:7cff999a7f5c 159 }
gohgwaja 0:7cff999a7f5c 160 }
gohgwaja 0:7cff999a7f5c 161 }
gohgwaja 0:7cff999a7f5c 162
gohgwaja 0:7cff999a7f5c 163 //error
gohgwaja 0:7cff999a7f5c 164 if(encoder_state == true)
gohgwaja 0:7cff999a7f5c 165 {
gohgwaja 0:7cff999a7f5c 166 pc.printf("Encoder error!\n\r");
gohgwaja 0:7cff999a7f5c 167 wait_ms(1000);
gohgwaja 0:7cff999a7f5c 168 while(1)
gohgwaja 0:7cff999a7f5c 169 {
gohgwaja 0:7cff999a7f5c 170 }
gohgwaja 0:7cff999a7f5c 171 }
gohgwaja 0:7cff999a7f5c 172 }
gohgwaja 0:7cff999a7f5c 173
gohgwaja 0:7cff999a7f5c 174 void encoder_check2()
gohgwaja 0:7cff999a7f5c 175 {
gohgwaja 0:7cff999a7f5c 176 encoder_reset_cnt();
gohgwaja 0:7cff999a7f5c 177
gohgwaja 0:7cff999a7f5c 178
gohgwaja 0:7cff999a7f5c 179 encoder_read();
gohgwaja 0:7cff999a7f5c 180
gohgwaja 0:7cff999a7f5c 181 for(int i=0;i<6;i++)
gohgwaja 0:7cff999a7f5c 182 {
gohgwaja 0:7cff999a7f5c 183 pc.printf("%8d", encoder_data[i]);
gohgwaja 0:7cff999a7f5c 184 }pc.printf("\r\n");
gohgwaja 0:7cff999a7f5c 185
gohgwaja 0:7cff999a7f5c 186 int encoder_check_move[6]={0,0,0,0,0,0};
gohgwaja 0:7cff999a7f5c 187
gohgwaja 0:7cff999a7f5c 188 for(int i=0; i<200;i++)
gohgwaja 0:7cff999a7f5c 189 {
gohgwaja 0:7cff999a7f5c 190 encoder_read();
gohgwaja 0:7cff999a7f5c 191
gohgwaja 0:7cff999a7f5c 192 for(int k=0;k<6;k++)
gohgwaja 0:7cff999a7f5c 193 {
gohgwaja 0:7cff999a7f5c 194 if(encoder_check[k]==false)
gohgwaja 0:7cff999a7f5c 195 {
gohgwaja 0:7cff999a7f5c 196 if(encoder_data[k]<-3)
gohgwaja 0:7cff999a7f5c 197 encoder_check[k]=true;
gohgwaja 0:7cff999a7f5c 198 if(encoder_data[k]>3)
gohgwaja 0:7cff999a7f5c 199 encoder_check[k]=true;
gohgwaja 0:7cff999a7f5c 200 encoder_check_move[k]=i;
gohgwaja 0:7cff999a7f5c 201 }
gohgwaja 0:7cff999a7f5c 202
gohgwaja 0:7cff999a7f5c 203
gohgwaja 0:7cff999a7f5c 204 if(encoder_check[k])
gohgwaja 0:7cff999a7f5c 205 {
gohgwaja 0:7cff999a7f5c 206 motor_power(k, 0);
gohgwaja 0:7cff999a7f5c 207 }
gohgwaja 0:7cff999a7f5c 208 else
gohgwaja 0:7cff999a7f5c 209 {
gohgwaja 0:7cff999a7f5c 210 motor_power(k, i);
gohgwaja 0:7cff999a7f5c 211 }
gohgwaja 0:7cff999a7f5c 212 }
gohgwaja 0:7cff999a7f5c 213 }
gohgwaja 0:7cff999a7f5c 214
gohgwaja 0:7cff999a7f5c 215
gohgwaja 0:7cff999a7f5c 216 for(int i=0;i<6;i++)
gohgwaja 0:7cff999a7f5c 217 motor_power(i, 0);
gohgwaja 0:7cff999a7f5c 218
gohgwaja 0:7cff999a7f5c 219 wait_ms(100);
gohgwaja 0:7cff999a7f5c 220
gohgwaja 0:7cff999a7f5c 221 for(int i=0;i<6;i++)
gohgwaja 0:7cff999a7f5c 222 {
gohgwaja 0:7cff999a7f5c 223 pc.printf("%8d", encoder_data[i]);
gohgwaja 0:7cff999a7f5c 224 }pc.printf("\r\n");
gohgwaja 0:7cff999a7f5c 225
gohgwaja 0:7cff999a7f5c 226 for(int i=0; i<200;i++)
gohgwaja 0:7cff999a7f5c 227 {
gohgwaja 0:7cff999a7f5c 228 encoder_read();
gohgwaja 0:7cff999a7f5c 229 for(int k=0;k<6;k++)
gohgwaja 0:7cff999a7f5c 230 {
gohgwaja 0:7cff999a7f5c 231 if(encoder_check[k]==false)
gohgwaja 0:7cff999a7f5c 232 motor_power(k, -i);
gohgwaja 0:7cff999a7f5c 233 }
gohgwaja 0:7cff999a7f5c 234 }
gohgwaja 0:7cff999a7f5c 235
gohgwaja 0:7cff999a7f5c 236 for(int i=0;i<6;i++)
gohgwaja 0:7cff999a7f5c 237 motor_power(i, 0);
gohgwaja 0:7cff999a7f5c 238
gohgwaja 0:7cff999a7f5c 239
gohgwaja 0:7cff999a7f5c 240 wait_ms(100);
gohgwaja 0:7cff999a7f5c 241
gohgwaja 0:7cff999a7f5c 242 encoder_read();
gohgwaja 0:7cff999a7f5c 243 for(int i=0;i<6;i++)
gohgwaja 0:7cff999a7f5c 244 {
gohgwaja 0:7cff999a7f5c 245 pc.printf("%8d", encoder_data[i]);
gohgwaja 0:7cff999a7f5c 246 }pc.printf("\r\n");
gohgwaja 0:7cff999a7f5c 247 for(int i=0;i<6;i++)
gohgwaja 0:7cff999a7f5c 248 {
gohgwaja 0:7cff999a7f5c 249 pc.printf("%8d", encoder_check_move[i]);
gohgwaja 0:7cff999a7f5c 250 }pc.printf("\r\n");
gohgwaja 0:7cff999a7f5c 251
gohgwaja 0:7cff999a7f5c 252 for(int i=0;i<6;i++)
gohgwaja 0:7cff999a7f5c 253 {
gohgwaja 0:7cff999a7f5c 254 if(encoder_check[i])
gohgwaja 0:7cff999a7f5c 255 pc.printf(" OK");
gohgwaja 0:7cff999a7f5c 256 else
gohgwaja 0:7cff999a7f5c 257 pc.printf(" ER");
gohgwaja 0:7cff999a7f5c 258
gohgwaja 0:7cff999a7f5c 259 motor_onoff[i]=encoder_check[i];
gohgwaja 0:7cff999a7f5c 260 }pc.printf("\r\n");
gohgwaja 0:7cff999a7f5c 261
gohgwaja 0:7cff999a7f5c 262
gohgwaja 0:7cff999a7f5c 263
gohgwaja 0:7cff999a7f5c 264 }
gohgwaja 0:7cff999a7f5c 265
gohgwaja 0:7cff999a7f5c 266 #endif
gohgwaja 0:7cff999a7f5c 267