test
Dependencies: mbed BufferedSerial LS7366LIB2 FastPWM
encoder.h
- Committer:
- lsh2205
- Date:
- 2020-04-23
- Revision:
- 0:e12eb40b9fef
File content as of revision 0:e12eb40b9fef:
#ifndef _ENCODER_H_ #define _ENCODER_H_ int ex_encoder_data[6]={0,}; int dif_encoder_data[6]={0,}; double filter_dif_encoder_data[6]={0,}; double filter_dif_encoder_co[6] = {0.01,0.01,0.01,0.01,0.01,0.01}; void filter_encoder_data() { for(int i=0; i<6;i++) { dif_encoder_data[i]= encoder_data[i] - ex_encoder_data[i]; 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]; ex_encoder_data[i] = encoder_data[i]; } } void encoder_read_raw() { encoder_data[0] = encoder1.read(); encoder_data[1] = encoder2.read(); encoder_data[2] = encoder3.read(); encoder_data[3] = encoder4.read(); encoder_data[4] = -encoder5.read(); encoder_data[5] = -encoder6.read(); } void encoder_read() { encoder_data[0] = encoder1.read(); encoder_data[1] = encoder2.read(); encoder_data[2] = encoder3.read(); encoder_data[3] = encoder4.read(); encoder_data[4] = -encoder5.read(); encoder_data[5] = -encoder6.read(); /* for(int i=0;i<6;i++) { pc.printf("%8d", encoder_data[i]); }pc.printf("\r\n"); */ filter_encoder_data(); } /* void reset_check() { } */ void reset_check() { 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) { motor_onoff[0]=false; motor_onoff[1]=false; motor_onoff[2]=false; motor_onoff[3]=false; motor_onoff[4]=false; motor_onoff[5]=false; while(1) { pwm1.write(0.4); pwm2.write(0.4); pwm3.write(0.4); pwm4.write(0.4); pwm5.write(0.4); pwm6.write(0.4); wait_ms(50); encoder_data[0] = encoder1.read(); if(encoder_data[0]!=-1) { NVIC_SystemReset(); } wait_ms(200); } } } void encoder_reset_cnt() { encoder1.LS7366_reset_counter(); encoder2.LS7366_reset_counter(); encoder3.LS7366_reset_counter(); encoder4.LS7366_reset_counter(); encoder5.LS7366_reset_counter(); encoder6.LS7366_reset_counter(); } void Time_To_Motor_Drive(int dir, double time_ms) { const int motor_percent[6] = {400, 400, 400, 350, -200, -200}; int motor_4_dir = 1; for(int i = 0; i < 6; i++) { if(i == 3) { motor_4_dir = -1; } else { motor_4_dir = 1; } motor_power(i, motor_percent[i] * dir * motor_4_dir); } wait_ms(time_ms); if(dir == -1) { for(int i = 4; i < 6; i++) { motor_power(i, 0); } wait_ms(time_ms); } } void encoder_check0() { int forward_enc[6] = {0,}; int reverse_enc[6] = {0,}; bool encoder_state = false; Time_To_Motor_Drive(0, 200); encoder_reset_cnt(); encoder_read(); for(int i = 0; i < 4; i++) { if(encoder_data[i] == -1) { encoder_state = true; pc.printf("%d : -1 \n\r", i); } } for(int i = 4; i < 6; i++) { if(encoder_data[i] == 1) { encoder_state = true; pc.printf("%d : -1 \n\r", i); } } if(encoder_state != true) { Time_To_Motor_Drive(1, 200); Time_To_Motor_Drive(0, 1000); encoder_read(); for(int i = 0; i < 6; i++) { forward_enc[i] = encoder_data[i];// * -1; } Time_To_Motor_Drive(-1, 250); Time_To_Motor_Drive(0, 1000); encoder_read(); for(int i = 0; i < 6; i++) { reverse_enc[i] = encoder_data[i];// * -1; if((forward_enc[i] > 100) || (forward_enc[i] > reverse_enc[i]) || (forward_enc[i] == 0 && reverse_enc[i] == 0)) { encoder_state = true; pc.printf("%d ", i); } } } //error if(encoder_state == true) { pc.printf("Encoder error!\n\r"); wait_ms(1000); while(1) { } } } void encoder_check2() { encoder_reset_cnt(); encoder_read(); for(int i=0;i<6;i++) { pc.printf("%8d", encoder_data[i]); }pc.printf("\r\n"); int encoder_check_move[6]={0,0,0,0,0,0}; for(int i=0; i<200;i++) { encoder_read(); for(int k=0;k<6;k++) { if(encoder_check[k]==false) { if(encoder_data[k]<-3) encoder_check[k]=true; if(encoder_data[k]>3) encoder_check[k]=true; encoder_check_move[k]=i; } if(encoder_check[k]) { motor_power(k, 0); } else { motor_power(k, i); } } } for(int i=0;i<6;i++) motor_power(i, 0); wait_ms(100); for(int i=0;i<6;i++) { pc.printf("%8d", encoder_data[i]); }pc.printf("\r\n"); for(int i=0; i<200;i++) { encoder_read(); for(int k=0;k<6;k++) { if(encoder_check[k]==false) motor_power(k, -i); } } for(int i=0;i<6;i++) motor_power(i, 0); wait_ms(100); encoder_read(); for(int i=0;i<6;i++) { pc.printf("%8d", encoder_data[i]); }pc.printf("\r\n"); for(int i=0;i<6;i++) { pc.printf("%8d", encoder_check_move[i]); }pc.printf("\r\n"); for(int i=0;i<6;i++) { if(encoder_check[i]) pc.printf(" OK"); else pc.printf(" ER"); motor_onoff[i]=encoder_check[i]; }pc.printf("\r\n"); } #endif