/* ------------------------------------------------------------------------ */ void update_motor_frequency(motor_t *motor){ if (motor->timer_update_flag){ uint32_t current_timer_count = motor->current_timer_count + TCNT0; if (current_timer_count > motor->last_timer_count){ motor->timer_diff = current_timer_count - motor->last_timer_count; }else{ motor->timer_diff = current_timer_count + MOTOR_MAX_COUNT - motor->last_timer_count; } /* Note: rps is approx: (CLK_FRQ/timer_diff)/(MOTOR_POLES/2) */ motor->frequency = ((float)(CLK_FRQ))/((float)(motor->timer_diff)); motor->rps = motor->frequency/((float)MOTOR_POLES/2); /* do maths */ #ifdef DEBUG usb_tx_decimal((motor->frequency)); usb_tx_char('\n'); usb_tx_char('\r'); usb_tx_decimal((motor->rps)); usb_tx_char('\n'); usb_tx_char('\r'); toggle(PORTD, 7); #endif motor->last_timer_count = current_timer_count; motor->timer_update_flag = 0; } return; };
void print_P(const char *s) { char c; while (1) { c = pgm_read_byte(s++); if (!c) break; if (c == '\n') usb_tx_char('\r'); usb_tx_char(c); } }
int main(void) { int i; char gains[PACKET_SIZE] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; char message[PACKET_SIZE] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; char L_string[PACKET_SIZE] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; char R_string[PACKET_SIZE] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; float P_gain=3, I_gain=0, D_gain=10, F_gain=1; // default gains volatile int L_des=0, R_des=0; float L_error=0, R_error=0, L_error_last, R_error_last, L_integral=0, R_integral=0, L_derivative, R_derivative; int L_out=0, R_out=0; init(); while(1){ // update velocity calculations if(L_delta){ L_velocity = L_velocity*(V_FILTER) + (125000/(float)L_delta) * (1-V_FILTER); // encoder counts per timer tick * N ticks/second } if(R_delta){ R_velocity = R_velocity*(V_FILTER) + (125000/(float)R_delta) * (1-V_FILTER); // encoder counts per timer tick * N ticks/second } L_error_last = L_error; L_error = (float)L_des - L_velocity; L_integral += L_error/1000; // assume I_gain given in milliunits L_derivative = L_error - L_error_last; L_out = (int)(P_gain*L_error + I_gain*L_integral + D_gain*L_derivative + F_gain*L_des); // velocity is negative if(L_out>=0){ OCR1B = L_out; set(PORTD,6); }else{ OCR1B = -L_out; clear(PORTD,6); } R_error_last = R_error; R_error = (float)R_des - R_velocity; R_integral += R_error/1000; // assume I_gain given in milliunits R_derivative = R_error - R_error_last; R_out = (int)(P_gain*R_error + I_gain*R_integral + D_gain*R_derivative + F_gain*R_des); // velocity is negative if(R_out>=0){ OCR1C = R_out; set(PORTD,7); }else{ OCR1C = -R_out; clear(PORTD,7); } // parse any incoming messages if(usb_rx_available()){ switch(usb_rx_char()){ case 'S': // set speeds message[0] = 'S'; for(i=1; i<9; i++){ while(!usb_rx_available()); // wait message[i] = (char)usb_rx_char(); } L_des = (unsigned int)(message[4] - '0') + (10 * (unsigned int)(message[3] - '0')) + (100 * (unsigned int)(message[2] - '0')); if(!(message[1]-'0')){ L_des = -L_des; } R_des = (unsigned int)(message[8] - '0') + (10 * (unsigned int)(message[7] - '0')) + (100 * (unsigned int)(message[6] - '0')); if(!(message[5]-'0')){ R_des = -R_des; } break; case 'G': gains[0] = 'G'; for(i=1; i<18; i++){ while(!usb_rx_available()); // wait message[i] = (char)usb_rx_char(); } P_gain = ((float)(message[4] - '0')/10) + (float)(message[2] - '0') + (10 * (float)(message[1]-'0')); I_gain = ((float)(message[8] - '0')/10) + (float)(message[6] - '0') + (10 * (float)(message[5]-'0')); D_gain = ((float)(message[12] - '0')/10) + (float)(message[10] - '0') + (10 * (float)(message[9]-'0')); F_gain = ((float)(message[16] - '0')/10) + (float)(message[14] - '0') + (10 * (float)(message[13]-'0')); break; case 'E': // request a sendback of the encoder delta for(i=0;i<PACKET_SIZE;i++){ // clear out the old string values L_string[i] = 0; R_string[i] = 0; } // grab the current values, which could otherwise be changing as we go through the code; L_encoder_temp = -L_encoder; R_encoder_temp = -R_encoder; // send the L wheel value to the dongle //L_encoder_delta = L_encoder_temp - L_encoder_last; //L_encoder_last = L_encoder_temp; ltoa(L_encoder_temp,&L_string[0],10); //L_string[0]='L'; // send the R wheel value to the dongle //R_encoder_delta = R_encoder_temp - R_encoder_last; //R_encoder_last = R_encoder_temp; ltoa(R_encoder_temp,&R_string[0],10); //R_string[0]='R'; for(i=0;i<PACKET_SIZE;i++){ usb_tx_char(L_string[i]); } usb_tx_string("\n"); for(i=0;i<PACKET_SIZE;i++){ usb_tx_char(R_string[i]); } usb_tx_string("\n"); break; default: break; } } } }
void phex1(unsigned char c) { usb_tx_char(c + ((c < 10) ? '0' : 'A' - 10)); }