// BROBOT EVO 2 by JJROBOTS // SELF BALANCE ARDUINO ROBOT WITH STEPPER MOTORS // License: GPL v2 // Control functions (PID controls, Steppers control...) // PD controller implementation(Proportional, derivative). DT in seconds float stabilityPDControl(float DT, float input, float setPoint, float Kp, float Kd) { float error; float output; error = setPoint - input; // Kd is implemented in two parts // The biggest one using only the input (sensor) part not the SetPoint input-input(t-1). // And the second using the setpoint to make it a bit more agressive setPoint-setPoint(t-1) float Kd_setPoint = constrain((setPoint - setPointOld), -8, 8); // We limit the input part... output = Kp * error + (Kd * Kd_setPoint - Kd * (input - PID_errorOld)) / DT; //Serial.print(Kd*(error-PID_errorOld));Serial.print("\t"); //PID_errorOld2 = PID_errorOld; PID_errorOld = input; // error for Kd is only the input component setPointOld = setPoint; return (output); } // PI controller implementation (Proportional, integral). DT in seconds float speedPIControl(float DT, int16_t input, int16_t setPoint, float Kp, float Ki) { int16_t error; float output; error = setPoint - input; PID_errorSum += constrain(error, -ITERM_MAX_ERROR, ITERM_MAX_ERROR); PID_errorSum = constrain(PID_errorSum, -ITERM_MAX, ITERM_MAX); //Serial.println(PID_errorSum); output = Kp * error + Ki * PID_errorSum * DT; // DT is in miliseconds... return (output); } float positionPDControl(long actualPos, long setPointPos, float Kpp, float Kdp, int16_t speedM) { float output; float P; P = constrain(Kpp * float(setPointPos - actualPos), -115, 115); // Limit command output = P + Kdp * float(speedM); return (output); } // TIMER 1 : STEPPER MOTOR1 SPEED CONTROL ISR(TIMER1_COMPA_vect) { if (dir_M1 == 0) // If we are not moving we dont generate a pulse return; // We generate 1us STEP pulse SET(PORTE, 6); // STEP MOTOR 1 //delay_1us(); if (dir_M1 > 0) steps1--; else steps1++; CLR(PORTE, 6); } // TIMER 3 : STEPPER MOTOR2 SPEED CONTROL ISR(TIMER3_COMPA_vect) { if (dir_M2 == 0) // If we are not moving we dont generate a pulse return; // We generate 1us STEP pulse SET(PORTD, 6); // STEP MOTOR 2 //delay_1us(); if (dir_M2 > 0) steps2--; else steps2++; CLR(PORTD, 6); } // Set speed of Stepper Motor1 // tspeed could be positive or negative (reverse) void setMotorSpeedM1(int16_t tspeed) { long timer_period; int16_t speed; // Limit max speed? // WE LIMIT MAX ACCELERATION of the motors if ((speed_M1 - tspeed) > MAX_ACCEL) speed_M1 -= MAX_ACCEL; else if ((speed_M1 - tspeed) < -MAX_ACCEL) speed_M1 += MAX_ACCEL; else speed_M1 = tspeed; #if MICROSTEPPING==16 speed = speed_M1 * 50; // Adjust factor from control output speed to real motor speed in steps/second #else speed = speed_M1 * 25; // 1/8 Microstepping #endif if (speed == 0) { timer_period = ZERO_SPEED; dir_M1 = 0; } else if (speed > 0) { timer_period = 2000000 / speed; // 2Mhz timer dir_M1 = 1; SET(PORTB, 4); // DIR Motor 1 (Forward) } else { timer_period = 2000000 / -speed; dir_M1 = -1; CLR(PORTB, 4); // Dir Motor 1 } if (timer_period > 65535) // Check for minimun speed (maximun period without overflow) timer_period = ZERO_SPEED; OCR1A = timer_period; // Check if we need to reset the timer... if (TCNT1 > OCR1A) TCNT1 = 0; } // Set speed of Stepper Motor2 // tspeed could be positive or negative (reverse) void setMotorSpeedM2(int16_t tspeed) { long timer_period; int16_t speed; // Limit max speed? // WE LIMIT MAX ACCELERATION of the motors if ((speed_M2 - tspeed) > MAX_ACCEL) speed_M2 -= MAX_ACCEL; else if ((speed_M2 - tspeed) < -MAX_ACCEL) speed_M2 += MAX_ACCEL; else speed_M2 = tspeed; #if MICROSTEPPING==16 speed = speed_M2 * 50; // Adjust factor from control output speed to real motor speed in steps/second #else speed = speed_M2 * 25; // 1/8 Microstepping #endif if (speed == 0) { timer_period = ZERO_SPEED; dir_M2 = 0; } else if (speed > 0) { timer_period = 2000000 / speed; // 2Mhz timer dir_M2 = 1; CLR(PORTC, 6); // Dir Motor2 (Forward) } else { timer_period = 2000000 / -speed; dir_M2 = -1; SET(PORTC, 6); // DIR Motor 2 } if (timer_period > 65535) // Check for minimun speed (maximun period without overflow) timer_period = ZERO_SPEED; OCR3A = timer_period; // Check if we need to reset the timer... if (TCNT3 > OCR3A) TCNT3 = 0; }