void rotate90left()
{
start_request = 1;
TransmitComm(update);
//stå still och kontrollera väggar runt roboten
for(long i = 0; i < 80000; i ++)
{
stopp();
}
storedValues[6] = 0;
while(turnisDone == 0)
{
start_request = 1;
TransmitSensor(turn); // Start gyro hos sensormodul
if (storedValues[6] != 1)
{
rotateleft();
}
else
{
start_request = 1;
TransmitSensor(turnstop); // kolla med sensormodul om vi roterat 90
stopp();
turnisDone = 1;
}
}
// uppdatera riktning på roboten
if(mydirection == 4)
{
mydirection = 1;
}
else
{
mydirection += 1;
}
turnisDone = 0;
storedValues[8] = mydirection;
//stå still och kontrollera väggar runt roboten
for(long i = 0; i < 80000; i ++)
{
stopp();
}
start_request = 1;
TransmitSensor(0);
TransmitComm(update);
posdistance = 0;
n = 0;
// Tållåt kontrol efter ö
controlisland = true;
}
void rotate90right()
{
start_request = 1;
TransmitComm(update);
for(long i = 0; i < 80000; i ++)
{
stopp();
}
storedValues[6] = 0;
while(turnisDone == 0)
{
start_request = 1;
TransmitSensor(turn);
if (storedValues[6] != 2)
{
rotateright();
}
else
{
start_request = 1;
TransmitSensor(turnstop);
stopp();
turnisDone = 1;
}
}
if(mydirection == 1)
{
mydirection = 4;
}
else
{
mydirection -= 1;
}
turnisDone = 0;
storedValues[8] = mydirection;
for(long i = 0; i < 80000; i ++)
{
stopp();
}
start_request = 1;
TransmitSensor(0);
TransmitComm(update);
posdistance = 0;
n = 0;
}
void MotorMaster::beginSerialInterface(int idelay) {
if (Serial.available() > 0) {
int inByte = Serial.read();
int speed; // Local variable
int delay = 100;
switch (inByte) {
case '1':
advance(delay);
break;
case '2':
back(delay);
break;
case '3':
turnR(delay);
break;
case '4':
turnL(delay);
break;
case '5':
slightL(delay);
break;
case '6':
slightR(delay);
break;
case '7':
stopp(delay);
break;
default:
break;
} // end switch
} // end if
delay(idelay);
}
L298N::L298N(int In1, int In2, int PWM)
{
pinIn1 = In1;
pinIn2 = In2;
pinPWM = PWM;
currDutyCycle = absDutyCycle = 0;
pinMode(pinIn1, OUTPUT);
pinMode(pinIn2, OUTPUT);
pinMode(pinPWM, OUTPUT);
stopp();
analogWrite(pinPWM, currDutyCycle);
}
void drivefromstill(float dist) //kör dist cm
{
distance=0;
dist = dist / 1.275625;
while (distance < dist * 1)
{
TransmitSensor(0);
driveF();
}
stopp();
}
void write_add(unsigned char address,unsigned char date)
{
start();
write_byte(0xa0);
respons();
write_byte(address);
respons();
write_byte(date);
respons();
stopp();
delay(1);
}
char read_add(unsigned char address)
{
unsigned char date;
start();
write_byte(0xa0);
respons();
write_byte(address);
respons();
start();
write_byte(0xa1);
respons();
date=read_byte();
stopp();
return date;
delay(1);
}
void firstlap()
{
if(myposX == startX && myposY == startY && mydirection == 1 && !start)
{
stopp();
onelap = 1;
start = 1;
straight();
start_request = 1;
TransmitComm(firstdone); // Berätta för komm att första varvet är klart
start_request = 0;
}
else
{
regulateright();
}
}
void regulateright()
{
transmit();
//REGLERING
//Omvandling till centimeter
sensor1r = shortsensor(storedValues[1]);
sensor2r = shortsensor(storedValues[2]);
sensorfront = shortsensor(storedValues[3]);
sensormeanr = ((sensor1r + sensor2r) / 2) + 4 ;
// Uppdatera kommunikationsmodulen med sensorvärden innan reglering börjar
if(firstRR == 1)
{
firstRR = 0;
TransmitComm(update);
stopp();
}
else
{
//till PD-reglering
Td = 5; //85000000; //128000000
K = 5;
if(sensorfront <= 15)
{
if(posdistance > 25)
{
updatepos();
}
straight();
rotate90left();
straight();
}
if(sensor1r < 25 && sensor2r < 25)
{
startregulate = 1;
PORTC = 0x01;
PORTD = 0x20;
rightpwm = speed + K * ((15 - sensormeanr)) + Td * (sensor2r - sensor1r);
leftpwm = speed - K * ((15 - sensormeanr)) - Td * (sensor2r - sensor1r);
if (rightpwm > 150)
{
OCR2B = 150;
}
else if(rightpwm < 0)
{
OCR2B = 0;
}
else
{
OCR2B = rightpwm;
}
if (leftpwm > 150)
{
OCR2A = 150;
}
else if (leftpwm < 0)
{
OCR2A = 0;
}
else
{
OCR2A = leftpwm;
}
}// Tappat konakt med bakhöger men inte framhöger
else if(sensor2r > 20 && sensor1r < 20 )
{
driveF();
}
else // Tappat kontakt med framhöger
{
//har vi reglerat mot vägg innan
if(startregulate == 1)
{
startregulate = 0;
drive(20); // Kör fram till mitten av ruta
updatepos();
rotate90right(); // sväng höger
drivefromstill(35); // Kör så att du har vägg att reglera mot
updatepos();
straight(); // korrigera om roboten står snett efter sväng
}
else
{
stopp();
}
}
}
}
void updatepos()
{
if (foundRFID == 0)
{
stopp();
start_request = 1;
TransmitSensor(RFID);
}
start = 0;
asm("");
switch(mydirection)
{
case (1): // X+
{
myposX += 1;
break;
}
case (2): // Y+
{
myposY += 1;
break;
}
case (3): // X-
{
myposX -= 1;
break;
}
case (4): // Y-
{
myposY -= 1;
break;
}
default:
{
}
}
// Nollställ avlagdstäcka till nästa position i rutnätet
posdistance = 0;
if (foundRFID == 0)
{
for(long i = 0; i < 160000; i++){}
for(long i = 0; i < 160000; i++){}
start_request = 1;
TransmitSensor(RFIDstop);
if (storedValues[7] == 1)
{
//foundRFID = 1;
RFIDX = myposX;
RFIDY = myposY;
}
}
else
{
storedValues[7] = 0;
}
// Tillåt att kolla vänster efter ö
controlisland = true;
//Tvinga en överföring på buss direkt
start_request = 1;
TransmitComm(update);
}
void remotecontrol()
{
cli();
DDRB = 0x00;
PINB = 0x00;
stopp();
while(1)
{
button = PINB & 0b00000111;
/*
1 = W
2 = D
3 = A
4 = S
5 = E
6 = Q
*/
setcursor(1);
print_on_lcd(button);
switch(button)
{
case (1)://Kör framåt, W
PORTC = 0x01; //Sätter båda DIR till 1
PORTD = 0x20;
OCR2A = 255; //PWM vänster
OCR2B = 244; //PWM höger
break;
case (4): //Backa, S
PORTC = 0x0; //Sätter båda DIR till 0
PORTD = 0x0;
OCR2A = 255;
OCR2B = 244;
break;
case (6): //Rotera vänster, Q
PORTC = 0x00; //DIR vänster till 0
PORTD = 0x20; //DIR höger till 1
OCR2A = 100;
OCR2B = 100;
break;
case (5): //Rotera höger, E
PORTC = 0x01;
PORTD = 0x00;
OCR2A = 100;
OCR2B = 100;
break;
case (3): //Sväng vänster, A
PORTC = 0x01;
PORTD = 0x20;
OCR2A = 120;
OCR2B = 255;
break;
case (2): //Sväng höger, D
PORTC = 0x01;
PORTD = 0x20;
OCR2A = 255;
OCR2B = 125;
break;
default:
stopp();
}
}
}
int main(void)
{
initiate_variables();
initiation();
int fjarrstyrt = (PIND & 0x01); //1 då roboten är i fjärrstyrt läge
initiate_request_timer();
if(fjarrstyrt == 1) // fjärrstyrt läge
{
for(long i = 0; i < 480000; i++){}
remotecontrol();
}
else // Autonom
{
MasterInit();
for(long i = 0; i < 480000; i++){}
while(home == 0)
{
TransmitSensor(0);
char i = 0;
if(n == 0)
{
i = 0;
}
else
{
i = 1;
}
if(posdistance > 31 + i) //40/2.55125)*0.9
{
updatepos();
if(n != 4)
{
n = n + 1;
}
else
{
n = 0;
}
}
if(!onelap)
{
firstlap();
}
else if(1) // else vill inte fungera på atmegan
{
secondlap();
}
}
stopp();
// Liten dans när den är på startpositionen
while(1)
{
rotate90right();
rotate90left();
}
}
return 0;
}