void main(void)
{
BYTE re1, re2;
BYTE a1, a2, a3, a4, a5, a6;
M48Init();
printf("\r\nM88PA demo\r\n");
pip();
#define T 1000
PORTB.1 = 1;
PORTB.2 = 1;
robotStop();
goFwd();
delay_ms(T);
goBack();
delay_ms(T);
goLeft();
delay_ms(T);
goRight();
delay_ms(T);
robotStop();
pip();
while (1)
{ //char c;
//c = getchar();
//printf("[%c] %3d\r\n",c, (int)c);
re1 = ReadByteADC(ADC_E1);
re2 = ReadByteADC(ADC_E2);
a1 = ReadByteADC(ADC_1);
a2 = ReadByteADC(ADC_2);
a3 = ReadByteADC(ADC_3);
a4 = ReadByteADC(ADC_4);
a5 = ReadByteADC(ADC_5);
a6 = ReadByteADC(ADC_6);
delay_ms(20);
printf("%4u %4u %4u %4u %4u %4u %4u %4u\r", re1, re2, a1, a2, a3, a4, a5, a6);
}
}
//Zero point turn
void RobotClass::rotateInPlace(float angle) {
lcd.clear();
float startAngle = globalCurrentPos.theta;
float temp;
//Positive angle = left turn
if (angle > 0) {
motors.setRightMotorSpeed(50);
motors.setLeftMotorSpeed(-50);
}
//negative angle = right turn
else if (angle < 0) {
motors.setRightMotorSpeed(-50);
motors.setLeftMotorSpeed(50);
}
//Check angle rotated agains magnitude of angle argument
while (fabs(globalCurrentPos.theta - startAngle) < fabs(angle) - 0.08 || fabs(globalCurrentPos.theta - startAngle) > fabs(angle) + 0.08) {
updatePosition(RobotClass::IMU_ENC);
}
//Stop robot turning once it gets to the correct angle
robotStop();
}
/** выполняем команду */
void action(RobotCommand* command) {
command->state = RUNNING;
Serial.print((long) command, HEX);
Serial.print(" - command(");
Serial.print(command->type);
Serial.print(")->param = ");
Serial.println(command->param);
noInterrupts();
switch (command->type) {
case MOTOR_FORWARD:
mShield.leftMotor(4, (int16_t) (command->param * FORWARD_FACTOR));
mShield.rightMotor(4, (int16_t) (command->param * FORWARD_FACTOR));
break;
case MOTOR_BACKWARD:
mShield.leftMotor(-4, (int16_t) (command->param * FORWARD_FACTOR));
mShield.rightMotor(-4, (int16_t) (command->param * FORWARD_FACTOR));
break;
case MOTOR_FORWARD_LEFT:
mShield.rightMotor(4, (int16_t) (command->param * ANGLE_FACTOR));
break;
case MOTOR_FORWARD_RIGHT:
mShield.leftMotor(4, (int16_t) (command->param * ANGLE_FACTOR));
break;
case MOTOR_LEFT:
mShield.rightMotor(4, (int16_t) (command->param * ANGLE_FACTOR / 2));
mShield.leftMotor(-4, (int16_t) (command->param * ANGLE_FACTOR / 2));
break;
case MOTOR_RIGHT:
mShield.rightMotor(-4, (int16_t)(command->param * ANGLE_FACTOR / 2));
mShield.leftMotor(4, (int16_t) (command->param * ANGLE_FACTOR / 2));
break;
case MOTOR_BACKWARD_LEFT:
mShield.leftMotor(-4, (int16_t) (command->param * ANGLE_FACTOR));
break;
case MOTOR_BACKWARD_RIGHT:
mShield.rightMotor(-4, (int16_t) (command->param * ANGLE_FACTOR));
break;
default:
break;
}
interrupts();
switch (command->type) {
case ROBOT_SCANING: // сканирование обстановки
scanSituation();
break;
case ROBOT_ANALYSE: // анализ ситуации и принятие решений
robotAnalyse();
break;
case ROBOT_STOP: // останавливаем всё
robotStop();
break;
default:
break;
}
mShield.waitBusy();
command->state = EMPTY;
}
void DebugProc(void)
{
BYTE re1, re2;
BYTE a1, a2, a3, a4, a5, a6;
printf("\r\nTEST REGIME\r\n");
// Отладочные телодвижения
// (проверка правильности подключения двигателей: вперед, назад, влево, вправо)
#define Time 500
goFwd(); delay_ms(Time);
goBack(); delay_ms(Time);
goLeft(); delay_ms(Time);
goRight(); delay_ms(Time);
robotStop();
pip();
goFastLeft(); delay_ms(Time);
goFastRight(); delay_ms(Time);
robotStop();
pip();
while (1)
{
re1 = ReadByteADC(ADC_E1);
re2 = ReadByteADC(ADC_E2);
a1 = ReadByteADC(ADC_1);
a2 = ReadByteADC(ADC_2);
a3 = ReadByteADC(ADC_3);
a4 = ReadByteADC(ADC_4);
a5 = ReadByteADC(ADC_5);
a6 = ReadByteADC(ADC_6);
delay_ms(20);
printf("(%4u %4u) (%4u %4u) %4u %4u %4u %4u\r", re1, re2, a1, a2, a3, a4, a5, a6);
}
}
void main(void)
{
int T; // Счетчик тактов для смены тактики поиска
int a; // Куда крутиться, 0 - влево, 1 - вправо, 2 - вперёд
BYTE cntOtval = 0; // Счетчик числа импульсов для опускания отвала
// Инициализация контроллера
M48Init();
// Port C initialization
// Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State6=T State5=P State4=P State3=P State2=P State1=T State0=T
PORTC=0x3C;
DDRC=0x00;
// Переопределяем ef_2 (D.4) как цифровой подтянутый вход
// Port D initialization
// Func7=Out Func6=Out Func5=Out Func4=In Func3=Out Func2=Out Func1=In Func0=In
// State7=0 State6=0 State5=0 State4=P State3=0 State2=0 State1=T State0=T
PORTD=0x10;
DDRD=0xEC;
// Скроостью управлять не будем. Выставляем по максиммуму
PORTB.1 = 1;
PORTB.2 = 1;
robotStop();
pip();
printf("\r\nSumo 88 RK-25/26MS Version 1.05\r\n\r\n");
//--------------------------------------------------------
// Переход в отладочный режим
// Для перехода в отладочный режим необходимо, чтоб перед включением датчики
// границы находились на светлом фоне
//--------------------------------------------------------
FotoL = ReadByteADC(6)>LimLeft;
FotoR = ReadByteADC(7)>LimRight;
if(FotoL && FotoR)
DebugProc();
robotStop();
//--------------------------------------------------------
// Ожидание сигнала START
//--------------------------------------------------------
while(SENSOR_START==0);
/***
// Выдача управляющах импульсов на сервомашинку, для опускания отвала
// Изображаем управляющий ШИМ
// Это займет около 10*(18+0.9) = 189 мс.
for(n=0;n<MAX_CNT_OTVAL;n++)
{
ef_1 = 0;
delay_us(18000); // Период импульсов. Вообще должно быть так: 20000 - <ширина импульса> = 20000-900 = 19100
ef_1 = 1;
delay_us(900); // Ширина импульса 900 мкс (0.9 мс) - крайнее положение
}
***/
ef_1 = 0;
T = 0;
a = 0;
//--------------------------------------------------------
// Основной цикл
//--------------------------------------------------------
while (1)
{
//------------------------------------------------------
// Опускаем отвал на ходу
//------------------------------------------------------
if(cntOtval<MAX_CNT_OTVAL)
{
cntOtval++;
//Выдаем импульс (с задержками)
delay_us(18000); // Период импульсов. Вообще должно быть так: 20000 - <ширина импульса> = 20000-900 = 19100
ef_1 = 1;
delay_us(900); // Ширина импульса 900 мкс (0.9 мс) - крайнее положение
ef_1 = 0;
}
// Реакция на сигнал СТОП
if(SENSOR_START==0)
{
robotStop();
pip();
while(1);
}
// Считываем сигналы датчиков
SharpSL = (sen_3==0);
SharpSR = (sen_4==0);
SharpFL = (sen_5==0);
SharpFR = (sen_6==0);
FotoL = ReadByteADC(6)>LimLeft;
FotoR = ReadByteADC(7)>LimRight;
//------------------------------------------------------
// Безусловные рефлексы
// Сначала и идет обработка самых приоритетных сигналов
//------------------------------------------------------
// Реакция на границу поля
if(FotoL)
{
// Отъезжаем назад
goBack();
delay_ms(100);
// Начинаем крутиться
goRight();
T = 0;
continue;
}
if(FotoR)
{
// Отъезжаем назад
goBack();
delay_ms(100);
// Начинаем крутиться
goLeft();
T = 0;
continue;
}
//------------------------------------------------------
// Общие действия
// Обнаружение противника передними датчиками
//------------------------------------------------------
if(SharpFL&&SharpFR)
{
goFwd();
T = 0;
continue;
}
if(SharpFL||SharpSL)
{
goLeft();
T = 0;
a = 0;
continue;
}
if(SharpFR||SharpSR)
{
goRight();
T = 0;
a = 1;
continue;
}
//------------------------------------------------------
// Никого не обнаружили
// Поиск противника
//------------------------------------------------------
if(a==0) goLeft();
if(a==1) goRight();
if(a==2) goFwd();
T=T+1;
if(T>MAXT) // Пора менять тактику
{
//pip();
T=0;
// Выбираем действие "случайным" образом
a=rand()%3; // Остаток от деления на 3 (деление по модулю 3)
}
}
}
void RobotClass::moveTo(RobotClass::rPosition waypoint, RobotClass::pathType path) {
//If moving in a straight line, path will be DIRECT
if (path == RobotClass::DIRECT) {
int heading;
//Give each motor an equal speed
motors.setLeftMotorSpeed(75);
motors.setRightMotorSpeed(75);
//Determine where the robot is
updatePosition(RobotClass::IMU_ENC);
//Distance and direction to objective
float distVec[2] = { 0,0 };
float distMag;
//Distance vector to waypoint
distVec[0] = waypoint.x - globalCurrentPos.x;
distVec[1] = waypoint.y - globalCurrentPos.y;
//Magnitude of distance vector
for (int i = 0; i < 2; i++) {
distMag += pow(distVec[i], 2);
}
distMag = sqrt(distMag);
//Not doing anything with the heading variable right now
if (abs(distVec[0]) > abs(distVec[1])) heading = 0;
else heading = 1;
//Angle to face directly at waypoint
float angleToWP;
float alphaAngle;
//Calculate angle to the waypoint from current position
alphaAngle = atan2(distVec[1], distVec[0]);
//Determine how far the robot should rotate
angleToWP = globalCurrentPos.theta - alphaAngle;
/*
If the current angle is larger than PI a positive angle
results from the atan2 function
However in a situation where the robot has an orientation of
theta = 3*PI/2 and the alphaAngle is PI, the robot should make a right zero
point turn, but it will make a left since the resultant angle is positive
So we multiply it by a negative 1
*/
if (globalCurrentPos.theta > M_PI) angleToWP = -angleToWP;
//Turn to face the waypoint
rotateInPlace(angleToWP);
leftWheelVel = 75;
rightWheelVel = 75;
float prevDistMag;
//Get within some distance of the target waypoint
while (distMag >= 2.50) {
motors.setLeftMotorSpeed(leftWheelVel);
motors.setRightMotorSpeed(leftWheelVel);
updatePosition(RobotClass::IMU_ENC);
//Adjust wheel speeds based on encoder count differences
//Helps maintain a straight line course
if (encoderLeftCounts < encoderRightCounts) {
leftWheelVel++;
rightWheelVel--;
}
else if (encoderLeftCounts > encoderRightCounts) {
rightWheelVel++;
leftWheelVel--;
}
distVec[0] = waypoint.x - globalCurrentPos.x;
distVec[1] = waypoint.y - globalCurrentPos.y;
prevDistMag = distMag;
for (int i = 0; i < 2; i++) {
distMag += pow(distVec[i], 2);
}
distMag = sqrt(distMag);
//Check the gyro readout and adjust wheel speeds to try and keep a 0 rotation
if(currGyro.zRot > 0.0) {
leftWheelVel++;
rightWheelVel--;
} else if(currGyro.zRot < 0.0){
leftWheelVel--;
rightWheelVel++;
}
}
//Stop at the waypoint
robotStop();
}
else if (path == RobotClass::CURVED) {
//Maintain the proper turn radius
lcd.clear();
//Calculate the turn velocities for a given turn radius
calcTurnVel(11.0f);
rightWheelVel = turnVel.right;
leftWheelVel = turnVel.leftVel;
float velRatio = leftWheelVel / rightWheelVel;
rightWheelVel = 50.0;
leftWheelVel = rightWheelVel*velRatio;
lcd.clear();
//Keep turning until robot is facing PI direction
while (globalCurrentPos.theta >= M_PI + 0.1 || globalCurrentPos.theta <= M_PI) {
//Set the motor speeds. The scalar multiples are used
//since the ration from the calcTurnVel() function
//doesn't seem to be quite right.
//The values sent to setLeftMotorSpeed are not
//actual wheel velocities, just a PWM value
motors.setLeftMotorSpeed(1.2*leftWheelVel);
motors.setRightMotorSpeed(0.9*rightWheelVel);
//Need to know where we are
updatePosition(RobotClass::IMU_ENC);
lcd.gotoXY(0, 0);
lcd.print(globalCurrentPos.theta);
float centerDist = (0.5f*rWheelCirc / encoderRes)*(encoderLeftCounts + encoderRightCounts);
}
//motors.setRightMotorSpeed(0);
//motors.setLeftMotorSpeed(0);
}
}
