uint16_t pid_find_max_encoder_value(void){
uint16_t enc_val = 0, prev_enc_val = -1;
motor_speed(75);
//Stop at left end
while(enc_val != prev_enc_val){
printf("In while\n");
enc_val = motor_encoder_read();
_delay_ms(100);
prev_enc_val = motor_encoder_read();
}
printf("Out of while\n");
motor_speed(0);
motor_encoder_reset();
//Stop at right end and set max
motor_speed(-75);
enc_val = 0, prev_enc_val = -1;
while(enc_val != prev_enc_val){
enc_val = motor_encoder_read();
_delay_ms(100);
prev_enc_val = motor_encoder_read();
}
motor_speed(0);
//Max value of encoder.
return enc_val;
}
//Reference position is a number between encoder min and encoder max.
float p_controller(float reference_position, float max_encoder_value){
//Convert slider to encoder values
//printf("Ref: %d\n", reference_position);
//New speed of the controller
int16_t error = reference_position-motor_encoder_read();
//printf("Enc: %d\n", motor_encoder_read());
//printf("Error: %d\n", error);
return ((k_p*error)*255)/max_encoder_value;
}
uint16_t pi_controller(uint16_t reference_position, uint16_t *error, uint16_t max_encoder_value){
//Convert slider to encoder values
reference_position *= (max_encoder_value/OVERFLOW_FIX)/255 * OVERFLOW_FIX;
uint16_t current_position = motor_encoder_read();
*error += reference_position-current_position;
//New speed of the controller
return -((float)(k_p*(reference_position-current_position) + k_i*(*error))*255)/max_encoder_value;
}
uint16_t controller_init(){
motor_speed(75);
uint16_t enc_val = 0, prev_enc_val = -1;
while(enc_val != prev_enc_val){
enc_val = motor_encoder_read();
_delay_ms(100);
prev_enc_val = motor_encoder_read();
}
motor_speed(0);
motor_encoder_reset();
motor_speed(-75);
enc_val = 0, prev_enc_val = -1;
while(enc_val != prev_enc_val){
enc_val = motor_encoder_read();
_delay_ms(100);
prev_enc_val = motor_encoder_read();
}
motor_speed(0);
//Find max value of encoder.
return enc_val;
}
//MOTOR SPEED NEEDS TO BE TUNED FOR INDIVIDUAL GAME BOARD
void motor_controller_calibrate_by_reset(){
uint8_t speed = 65;
motor_init();
uint16_t position;
uint16_t prev_position;
motor_speed(speed);
motor_direction(right);
_delay_ms(150);
position = motor_encoder_read();
//go right until stopped, then set encoder to zero
while(position != prev_position){
position = motor_encoder_read();
_delay_ms(100);
prev_position = position;
position = motor_encoder_read();
printf("position: %u\tPrev: %u\n", position, prev_position);
}
motor_encoder_reset();
motor_speed(speed);
motor_direction(left);
_delay_ms(150);
//go left until stopped, set max left to current position
do{
position = motor_encoder_read();
_delay_ms(100);
prev_position = position;
position = motor_encoder_read();
printf("position: %d\n", position);
} while(position != prev_position);
max_left = position;
motor_speed(0);
motor_speed(speed);
motor_direction(right);
_delay_ms(100);
while(motor_encoder_read() > 4500){
_delay_ms(100);
}
}
