void initializeRobot (RobotModel robot)
{
SensorType[soundSensor] = sensorSoundDB;
SensorType[touchSensor] = sensorTouch;
// Tells the motor to break (rather than coast) when stopping
bFloatDuringInactiveMotorPWM = false;
// Set the logical motor direction
switch (robot)
{
case RMTaskbot:
case RMDragRacer:
case RMSammyDavisJr:
case RMMichaelJackson:
setMotorDirection(backwardMotorDirection);
break;
case RMRobotEducator:
case RMDomabot:
case RMCiara:
case RMJLo:
case RMJustinTimberlake:
case RMMadonna:
default:
setMotorDirection(forwardMotorDirection);
break;
}
eraseDisplay();
switch (robot)
{
case RMRobotEducator: nxtDisplayCenteredTextLine(0, "Robot Educator"); break;
case RMTaskbot: nxtDisplayCenteredTextLine(0, "Taskbot"); break;
case RMDomabot: nxtDisplayCenteredTextLine(0, "Domabot"); break;
case RMDragRacer: nxtDisplayCenteredTextLine(0, "Drag Racer"); break;
case RMJLo: nxtDisplayCenteredTextLine(0, "J. Lo"); break;
case RMJustinTimberlake: nxtDisplayCenteredTextLine(0, "J. Timberlake"); break;
case RMMadonna: nxtDisplayCenteredTextLine(0, "Madonna"); break;
case RMSammyDavisJr: nxtDisplayCenteredTextLine(0, "Sammy Davis Jr."); break;
case RMCiara: nxtDisplayCenteredTextLine(0, "Ciara"); break;
case RMMichaelJackson: nxtDisplayCenteredTextLine(0, "Michael Jackson"); break;
default: nxtDisplayCenteredTextLine(0, "Unknown (%d)", robot); break;
}
nxtDisplayCenteredTextLine(2, "Initializing");
enableSpeedCtrl();
wait1Msec(5000);
}
void
motor_task(void)
{
for(;;){
if(s_front < MIN_DISTANCE && s_left >MAX_DISTANCE)
{
r_dir= FORWARD;
r_duty = 255;
l_dir = BACKWARD;
l_duty = 255;
} else if (s_front < MIN_DISTANCE && s_right>MAX_DISTANCE) {
r_dir = BACKWARD;
r_duty = 255;
l_dir= FORWARD;
l_duty = 255;
} else if (s_left < MIN_DISTANCE && s_right > MAX_DISTNACE){
l_dir = FORWARD;
l_duty = 255;
r_duty = 0;
} else if (s_right<MIN_DISTANCE && s_left > MAX_DISTANCE){
r_dir = BACKWARD;
r_duty = 255;
l_duty = 0;
} else {
Signal_Event(stop);
}
setMotorDirection(&rightMotor,r_dir;
setMotorDuty(&rightMotor, r_duty);
setMotorDirection(&leftMotor,l_dir);
setMotorDuty(&leftMotor, l_duty);
}
}
//"square" motion pattern
void Asuro::driveSquare(int size, int speed)
{
setMotorSpeed(speed, speed);
//forwards
setMotorDirection (1, 1);
delay (size);
//right
setMotorDirection (1,0);
delay (size);
//backwards
setMotorDirection (0, 0);
delay (size);
//left
setMotorDirection (0,1);
delay (size);
setMotorSpeed(0, 0);
}
void cn_debug_serial(uword command) {
sword pwm;
unsigned static short int motor = MOTOR3;
switch(command) {
case 'a':
setMotorDirection(motor, DIRECTION_CCW);
break;
case 's':
setMotorDirection(motor, DIRECTION_CW);
break;
case '1':
motor = MOTOR1;
break;
case '2':
motor = MOTOR2;
break;
case '3':
motor = MOTOR3;
break;
case 'u': // PWM up
pwm = getMotorPWM(motor);
pwm += 7;
setMotorPWM(motor, pwm);
break;
case 'd': // PWM down
pwm = getMotorPWM(motor);
pwm -= 7;
if(pwm < ZERO_PWM) pwm = ZERO_PWM;
setMotorPWM(motor, pwm);
break;
case 't': // toggle direction
toggleMotorDirection(motor);
break;
case ' ': // PWM off
stopAllMotors();
break;
default:
break;
}
}
int
main(int argc, char *argv[])
{
motor_t rightMotor = {
&PORTB, &DDRB, PORTB7, // PWM
&PORTC, &DDRC, PORTC1, // Direction
&OCR0A // Top value
};
motor_t leftMotor = {
&PORTD, &DDRD, PORTD0, // PWM
&PORTB, &DDRB, PORTB0, // Direction
&OCR0B // Top value
};
//uint8_t sonarDistance = 0;
cli();
NO_CLK_PRESCALE();
//uart_init();
radio_init(HOV1_ADDRESS, RECEIVE_MODE);
sonar_init();
motorInit(&rightMotor);
motorInit(&leftMotor);
pwmInit();
sei();
stop = Event_Init();
sendPing();
while(!receivedInit);
Task_Create((void*)(&motor_task),0, PERIODIC, MOTOR);
Task_Create((void*)(&fire_sonar),0, PERIODIC, FIRE);
Task_Create((void*)(&listen_sonar),0,PERIODIC, LISTEN);
Task_Create((void*)(&sendRadio),0,PERIODIC, RADIO);
Task_Create((void*)(&stopSystem), 0,SYSTEM, STOP);
setMotorDirection(&rightMotor, FORWARD);
setMotorDirection(&leftMotor, FORWARD);
return 0;
}
void toggleMotorDirection(ubyte motor) {
bool direction;
direction = getMotorDirection(motor);
switch(direction) {
case DIRECTION_CW:
direction = DIRECTION_CCW;
break;
case DIRECTION_CCW:
direction = DIRECTION_CW;
break;
default:
break;
}
setMotorDirection(motor, direction);
}
void Engine::changeMotorState(unsigned char motorDirection, unsigned char motorSpeedA , unsigned char motorSpeedB)
{
setMotorSpeedAB(motorSpeedA, motorSpeedB);
delay(10); // required delay
setMotorDirection(motorDirection);
}
void pimobileSetRightMotorDirection( short direction ) {
setMotorDirection( GPIO_RIGHT_A, GPIO_RIGHT_B, direction );
}
void pimobileSetLeftMotorDirection( short direction ) {
setMotorDirection( GPIO_LEFT_A, GPIO_LEFT_B, direction );
}
int main(void){
SYSTEMConfigPerformance(10000000);
enableInterrupts();
initTimer1();
initTimer2();
initTimer45();
initSW1();
initLCD();
initPWM();
initADC();
setMotorDirection(M1, 1);
setMotorDirection(M2, 1);
while(1){ //Lab3 Part1
switch(state){
case forward:
prevstate = forward;
ADCbuffer = getADCbuffer();
if((dispVolt < ADCbuffer) && ((dispVolt + 1) <= ADCbuffer)){//to reduce excessive LCD updates
printVoltage(ADCbuffer);
dispVolt = ADCbuffer;
}
else if((dispVolt > ADCbuffer) && ((dispVolt - 1) >= ADCbuffer)){
printVoltage(ADCbuffer);
dispVolt = ADCbuffer;
}
moveCursorLCD(1,0);
printStringLCD("forward ");
if(remap == 1){
setMotorDirection(M1,1);
setMotorDirection(M2,1);
delayUs(1000);
remap = 0;
}
setMotorSpeed(ADCbuffer, direction);
break;
case backward:
prevstate = backward;
ADCbuffer = getADCbuffer();
if((dispVolt < ADCbuffer) && ((dispVolt + 1) <= ADCbuffer)){//to reduce excessive LCD updates
printVoltage(ADCbuffer);
dispVolt = ADCbuffer;
}
else if((dispVolt > ADCbuffer) && ((dispVolt - 1) >= ADCbuffer)){
printVoltage(ADCbuffer);
dispVolt = ADCbuffer;
}
moveCursorLCD(1,0);
printStringLCD("backward");
if(remap == 1){
setMotorDirection(M1,0);
setMotorDirection(M2,0);
delayUs(1000);
remap = 0;
}
setMotorSpeed(ADCbuffer, direction);
break;
case idle:
unmapPins();
ADCbuffer = getADCbuffer();
if((dispVolt < ADCbuffer) && ((dispVolt + 1) <= ADCbuffer)){//to reduce excessive LCD updates
printVoltage(ADCbuffer);
dispVolt = ADCbuffer;
}
else if((dispVolt > ADCbuffer) && ((dispVolt - 1) >= ADCbuffer)){
printVoltage(ADCbuffer);
dispVolt = ADCbuffer;
}
moveCursorLCD(1,0);
printStringLCD("Idle ");
delayUs(1000);
break;
}
}
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)
{
//
}
}
}