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) { // } } }