void moveServo( uint8_t portions ){
int i;
for(i=0; i<portions; i++){
setServoAngle(0,135);
setServoAngle(0,0);
}
}
bool openCubesCatcher()
{
setServoAngle((uint8_t)ID_RIGHT, (uint16_t)OPEN_ANG_RIGHT);
setServoAngle((uint8_t)ID_LEFT, (uint16_t)OPEN_ANG_LEFT);
softDelay(500);
// duplicate
setServoAngle((uint8_t)ID_RIGHT, (uint16_t)OPEN_ANG_RIGHT);
setServoAngle((uint8_t)ID_LEFT, (uint16_t)OPEN_ANG_LEFT);
return 0;
}
void initAll(){
SysTick_Config(SystemCoreClock / 1000);
lcd_init();
lcd_home();
lcd_cls();
initPWM(PWM_TIME_PERIOD);
setServoAngle(0,0);
UARTInit(BAUD_RATE);
ds1307Init();
eeprom_24aaInit();
eeprom24aaMemoryCheck();
initButtons();
}
bool closeCubesCatcher(uint8_t *numberOfCubesCatched)
{
setServoAngle((uint8_t)ID_RIGHT, (uint16_t)CLOSED_ANG_RIGHT);
setServoAngle((uint8_t)ID_LEFT, (uint16_t)CLOSED_ANG_LEFT);
softDelay(500);
// duplicate
setServoAngle((uint8_t)ID_RIGHT, (uint16_t)CLOSED_ANG_RIGHT);
setServoAngle((uint8_t)ID_LEFT, (uint16_t)CLOSED_ANG_LEFT);
softDelay(20000000);
while((prev_right_servo_angle != right_servo_angle) || (prev_left_servo_angle != left_servo_angle))
{
prev_right_servo_angle = right_servo_angle;
prev_left_servo_angle = left_servo_angle;
getServoAngle((uint8_t)ID_RIGHT, &right_servo_angle);
softDelay(1000);
getServoAngle((uint8_t)ID_LEFT, &left_servo_angle);
}
float difference = right_servo_angle - left_servo_angle;
if ((difference > 1) && (difference <= 15))
{
*numberOfCubesCatched = 1;
}
else if ((difference > 15))
{
*numberOfCubesCatched = 2;
}
else
*numberOfCubesCatched = 0; // no cubes were caught or number of cubes is unknown
right_servo_angle = 0;
left_servo_angle = 0;
prev_right_servo_angle = 1;
prev_left_servo_angle = 1;
return 0;
}
void setServoAngleInt2( int angle ) {
float fAngle = (float)angle;
float setAngle = 0.0;
float fPercent = 0.0;
float angleSpan = 0.0;
if( fAngle < 0.0 ) {
fPercent = fAngle / ( SERVO_ANGLE_INT_MID_INDEX - SERVO_ANGLE_INT_MIN_INDEX );
angleSpan = fPercent * ( SERVO_ANGLE_MID_INDEX - SERVO_ANGLE_MIN_INDEX );
setAngle = SERVO_ANGLE_MID_INDEX + angleSpan;
} else if( fAngle > 0.0 ) {
fPercent = fAngle / ( SERVO_ANGLE_INT_MAX_INDEX - SERVO_ANGLE_INT_MID_INDEX );
angleSpan = fPercent * ( SERVO_ANGLE_MAX_INDEX - SERVO_ANGLE_MID_INDEX );
setAngle = SERVO_ANGLE_MID_INDEX + angleSpan;
} else {
setAngle = 0.0;
}
setServoAngle( setAngle );
}
bool closeTower(int8_t ID) // Закрыть манипулятор
{
setServoAngle(ID, CLOSED_ANG);
return 0;
}
bool openTower(int8_t ID) // Открыть манипулятор
{
setServoAngle(ID, OPEN_ANG);
return 0;
}
int main(void)
{
//Calibração do sensor
/*configPWM();
setServoAngle(0);
while(1){
}*/
double dt = 0.0;
double curr_err = 0;
float PID_Input = 0;
float PID_Output = 0;
int i_PID_Input = 0;
int i_Output = 0;
char c_PID_Input[4];
char c_Output[4];
char c_adc_value[4];
float gain = -20.0;
ADC_config();
ADC_Start();
USART_config();
configPWM();
setServoAngle(0);
ConfigSerie_BT();
pid_start();
pid_timer_config();
configMotorPWM(500);
setMotorDirection(1);
duty_cycle = 10;
while(1)
{
if(mode == 1)
{
if(all_read == 1)
{
PID_Input = Centroid_Algorithm();
curr_err = (3.5 - PID_Input);
dt = TCNT5;
dt = (dt*16)/1000000;
pid_update(ptr, curr_err,dt);
PID_Output = PID.control;
itoa(i_Output, c_Output, 10);
SendString((char*)"\n-------------------\n");
SendString((char*)"\nSaída_PID 1: ");
SendString(c_Output);
PID_Output = PID_Output*gain;
if(PID_Output > 75)
PID_Output = 75;
if(PID_Output < -75)
PID_Output = -75;
setServoAngle(PID_Output);
i_PID_Input = (int) PID_Input;
i_Output = (int) PID_Output;
itoa(i_PID_Input, c_PID_Input ,10);
itoa(i_Output, c_Output, 10);
SendString((char*)"\nLeituras: ");
for(int i = 0; i < 8; i++)
{
itoa(adc_value[i], c_adc_value, 10);
SendString(c_adc_value);
UsartSendByte(' ');
}
SendString((char*)"\nEntrada PID: ");
SendString(c_PID_Input);
SendString((char*)"\nSaída_PID 2: ");
SendString(c_Output);
all_read = 0;
ADMUX &= 0XE0; //Limpa ADMUX
ADMUX |= channel; //Selecciona o canal 0
ADCSRA |= (1<<ADSC); //Inicia conversão
}
}
if(mode == 0)
{
//
}
}
}
