/* ------------------------------------------------------------------------ */
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));
}
