Kod till AGV
The snippet can be accessed without any authentication.
Authored by
Herman Bermell
29.27 KiB
#include "mbed.h"
#include "Bas.h"
/******************
* MAIN *
******************\
// main() runs in its own thread in the OS
int main() {
//Setup för seriell kommunikation med dator
Serial pc(USBTX,USBRX);
//Sätt baudrate till 9600
pc.baud(9600);
camCom kamera(PC_6,PA_12);
char list[100][4];
//Sätt hela list till \0
memset(list, '\0', sizeof(list));
// Skapar objekt för bilens data
// med start o (0,0) och riktning höger(x++)
// statart timer för senast uppdaterad koordinat
carData D1(0,0,1);
carData* pD1 = &D1;
D1.updateIntervallTimer.start(); //sätt i constructor istället?
//Skapar motorobejekt
motor L(PA_7, PA_11, PC_5); //PC_5 SKA VARA PA_12 MED HUVUDKORT
motor R(PA_6, PC_7, PC_9);
motor* pL = &L;
motor* pR = &R;
HC05 BT(PA_9,PA_10, pD1, pL, pR);
yttreIR LL{PC_8, pD1};
yttreIR RR{PB_5, pD1};
sensorRamp Ramp(&D1);
float SpeedL = 0;
float SpeedR = 0;
int xTarget = 0;
int yTarget = 0;
int index = 0;
char turn;
char nodeType;
int listLoc = 0;
Timer stopTime;
float old_data;
float data;
LED lampa(pD1);
/*****************************
* Vänta på koordinatlista *
******************************/
D1.state = 'V';
while(true){
if(BT.getCoord(list)){
printf("Koordinater mottagna\n");
for(int i = 0; i<100;i++){
if(list[i][0] == '\0'){
break;
}
printf("%c,%c %c %c \n", list[i][0],list[i][1],list[i][2],list[i][3]);
}
BT.sendChar('K');
break;
}
}
/****************************
* Vänta på startkommando *
*****************************/
while(true){
if(BT.getComm() == 'S'){
printf("Startar...\n");
break;
}
}
/****************************
* Kör *
*****************************/
D1.state = 'R'; // Sätt status Körning
BT.startStatus(); //Starta interrupt för status
BT.StartStopInterupt();//Starta serialInterrupt för stoppknapp
while (true) {
//Verkar inte funka. Behövs den?
// if (D1.outofBounds){
// printf("Utanför tillåtet område! :(\n");
// D1.state = 'q';
// L.stop();
// R.stop();
// break;
// }
//Läser in data för nästa nod
xTarget = charToInt(list[listLoc][0]);
yTarget = charToInt(list[listLoc][1]);
nodeType = list[listLoc][2];
turn = list[listLoc][3];
//Kör tills framme vid noden
while(D1.x != xTarget || D1.y != yTarget){
data = kamera.data();
if(data != old_data){
old_data = data;
//Om AGV är 2 rutor eller fler ifrån målet ska AGV köra snabbt
if((D1.h == 1 || D1.h == 4 ) && (abs(D1.x - xTarget) >= 2)){
Ramp.motorPIDcontrolFast(SpeedL, SpeedR, data);
}
else if ((D1.h == 2 || D1.h == 3 ) && (abs(D1.y - yTarget) >= 2)) {
Ramp.motorPIDcontrolFast(SpeedL, SpeedR, data);
}
//Köra långsamt för att svängen ska bli bra
else {
Ramp.motorPIDcontrolSlow(SpeedL, SpeedR, data);
}
}
//Skriv den uträknade hastigheten till motorerna
R.runBasic(SpeedR);
L.runBasic(SpeedL);
}
//Sväng samt uppdatering av riktning
if(Ramp.turn(turn, &R, &L, LL.Turn, RR.Turn)){
D1.h = getHeading(D1.h, turn);
}
//Om noden är en plocknod ska vi stanna.
//Funkar endast vid första plocknoden.
if(nodeType == 's'){
stopTime.start();
L.stop();
R.stop();
D1.state = 'P';
while(true){
if(stopTime.read() > 3){
stopTime.stop();
stopTime.reset();
printf("Färdig med plock \n");
break;
}
}
}
//Ändrar på lite states
LL.Turn = false;
RR.Turn = false;
D1.state = 'R';
//Inkrementera list index
listLoc++;
//Kollar om slutet av listan är nåd
if(list[listLoc][0] == '\0'){
D1.state = 'm';
BT.endStatus();
printf("Körorder klar!");
L.stop();
R.stop();
return 0;
}
}
return 0;
}
/******************
*Kommunikation.h*
******************\
#include "mbed.h"
#include "linje.h"
//Start Communication
#define SC 'K'
//End Communication
#define EC 'E'
//Stop
#define STOP 'X'
//GO
#define GO 'S'
//Klass för bluetooth-kommunikation
class HC05 {
public:
RawSerial BT; //UART serial
HC05(PinName tx, PinName rx, carData *data, motor *adressl, motor *adressR);//Constructor
bool getCoord(char list[][4]); //Function som hämtar koordinater
char getComm(); //Tar emot chars för styrning
void sendStatus(); //Skickar aktuell status
void sendChar(const char C); //Skickar en char
void statusISR(); //ISR för att skicka status i timer-intervall
void getPID(float &Kp, float &Kd); //För att ändra PID-värden över BT
void startStatus();
void endStatus();
void serialStop();//Funktion för stopp
void StartStopInterupt(); //Funktion som kallar på serialstop
private:
//Index på array
int indexR;
int indexC;
carData *cData; //Pointer till aktuell bildata
Ticker status; //Status-interrupt
motor* adrL; //Pointer till motor Left
motor* adrR; //Pointer till Motor Höger
char btChar; //char som läser från buffer
bool started; //Commnunication started
bool ended; //Communacation ended
bool PID; //Varliabel för getPID
char PIDprev; //Varliabel för getPID
const float KpReg = 0.01; //Skalare för Kp
const float KdReg = 0.1; //Skalare för Kd
};
//Kamera-kommunikation, under konstruktion.
class camCom {
//PA_3
//PA_2
public:
RawSerial cam; //UART serial-objekt
camCom(PinName tx, PinName rx); //Constructor
void send(const char snd); //Skicka data
float data(); //läs mottagen data
bool ended;
private:
void get(); //Ta emot data, startas i interrupt
char floats[10]; //Array som lagrar mottagen data
char camChar; //läser data
int index; //index på array
};
/******************
*Kommunikation.cpp*
******************\
#include "kommunikation.h"
//Constructor för HC05-klass
//initierar en Serial-type som heter BT.
HC05::HC05(PinName tx, PinName rx, carData *data, motor *adressL, motor*adressR)
: BT(tx,rx) {
//sätter baud till 9600. Kanske bör ökas i framtiden?
BT.baud(9600);
//8-bit kommunikation utan parity-bits, en stop-bit
BT.format(8,SerialBase::None,1);
//Index för koordinat-lista
indexR = 0;
indexC = 0;
started = false;
ended = false;
cData = data;
adrL = adressL;
adrR = adressR;
}
//Funktion för att ta emot koordinater från ÖS
//Ett 2D-array skickas som argument och fylls i funktionen
void HC05::startStatus(){
//Timerinterrupt för att skicka status
status.attach(callback(this, &HC05::sendStatus), 0.5);
}
void HC05::StartStopInterupt(){
BT.attach(callback(this,&HC05::serialStop),Serial::RxIrq);
}
void HC05::serialStop()
{
if(BT.readable()){
if(BT.getc() == 'X'){
adrL->stop();
adrR->stop();
adrL->runnable = false;
adrR->runnable = false;
cData->state = 'Q';
}
}
}
void HC05::endStatus(){
status.detach();
}
bool HC05::getCoord(char list[][4]) {
started = false;
ended = false;
//Kontroll om buffern kan läsas ifrån
if (BT.readable()){
//Disable interrupts.
__disable_irq();
while(true){
btChar = BT.getc();
//Startchar
if(btChar == SC) {
started = true;
//Index reset
indexC = 0;
indexR = 0;
}
//Endchar
else if (btChar == EC) {
ended = true;
break;
}
//Mata in i list annars
else{
if(indexC < 4) {
//Mata in data i list array från btChar
list[indexR][indexC] = btChar;
//Nästa kolumn
indexC++;
}
else {
//Börja om på kolumn 1
indexC = 0;
//Nästa rad;
indexR++;
//Mata in data på första platsen i nya raden
list[indexR][indexC] = btChar;
//Nästa kolumn
indexC++;
}
}
}
//Enable interrupts
__enable_irq();
}
//Kontroll om allt tagits emot korrekt
if(started && ended){
return true;
}
else {
return false;
}
}
void HC05::sendChar(const char C){
if(BT.writeable()){
BT.putc(C);
}
}
//Funktion för att skicka aktuell status till ÖS.
void HC05::sendStatus(){
//KOntroll om buffern kan skrivas till
if(BT.writeable()){
//Loopa igenom list för att skicka all aktuell status
if(cData->x >= 10){
BT.putc((cData->x + 'W'));
}
else{
BT.putc((cData->x + '0'));
}
BT.putc((cData->y + '0'));
if(cData->h == 1){
BT.putc('R');
}
else if(cData->h == 2){
BT.putc('D');
}
else if(cData->h == 3){
BT.putc('U');
}
else if(cData->h == 4){
BT.putc('L');
}
if(cData-> state == 'T' || cData-> state == 'R'){
BT.putc('M');
}
else{
BT.putc(cData->state);
}
BT.putc('\n');
}
}
//Läser in en char från bluetooth. just nu endast S och X
char HC05::getComm() {
//Kontroll om buffern kan läsas ifrån
if(BT.readable()) {
btChar = BT.getc();
//Kontrollerar om det är någon av det chars som tillåts
if(btChar == GO || btChar == STOP){
return btChar;
}
else {
return '\0';
}
}
else {
return '\0';
}
}
void HC05::getPID(float &Kp, float &Kd){
if(BT.readable()){
while(true){
btChar = BT.getc();
if(btChar == 'P'){
PIDprev = btChar;
}
if(btChar == '+' && PIDprev == 'P'){
Kp += KpReg;
PID = true;
break;
}
if(btChar == '-' && PIDprev == 'P'){
Kp -= KpReg;
PID = true;
break;
}
if(btChar == 'D'){
PIDprev = btChar;
}
if(btChar == '+' && PIDprev == 'D'){
Kd += KdReg;
PID = true;
break;
}
if(btChar == '-' && PIDprev == 'D'){
Kd -= KdReg;
PID = true;
break;
}
}
if(BT.writeable()){
if (PID == true && PIDprev == 'P'){
BT.putc('D');
BT.putc('i');
BT.putc('n');
BT.putc('c');
BT.putc('\n');
}
if (PID == true && PIDprev == 'P'){
BT.putc('P');
BT.putc('i');
BT.putc('n');
BT.putc('c');
BT.putc('\n');
}
}
}
}
camCom::camCom(PinName tx, PinName rx) : cam(tx,rx) {
cam.baud(19200);
index = 0;
//Interrupt när RX-buffer blir fylld
cam.attach(callback(this, &camCom::get), Serial::RxIrq);
}
void camCom::send(const char snd){
//Skickar en char
if(cam.writeable()) {
cam.putc(snd);
}
return;
}
void camCom::get(){
if(cam.readable()){
camChar = cam.getc();
//om start-char
if(camChar == SC){
ended = false;
index = 0;
}
//om end-char
else if(camChar == EC){
ended = true;
}
//Mata in data i array
else if (index<10){
floats[index] = camChar;
index++;
}
}
}
float camCom::data(){
//Konverterar array med chars till float
return atof(floats);
}
/******************
* Motorstyrning.h *
******************\
#include "mbed.h"
#include "PID.h"
class motor {
public:
//constructor
motor(PinName PWM, PinName fwd, PinName bwd);
//Kör motor
void runBasic(float speed);
//Stannar motor
void stop();
bool runnable;
private:
PwmOut motorPWM; //PWM för motor
DigitalOut FWDpin; //PC_9 för PA_6, PA_11 för PA_7
DigitalOut BWDpin; //PC_7 för PA_6, PA_12 för PA_7
//InterruptIn Pulses; //Interrupt för att räkna puls
};
/******************
*Motorstyrning.cpp*
******************\
#include <motorstyrning.h>
motor::motor(PinName PWM, PinName fwd, PinName bwd)
: motorPWM(PWM), FWDpin(fwd), BWDpin(bwd){
//Sätter periodtiden för PWM-signalen.
motorPWM.period_us(50);
//Sätter båda riktningspinsen till 0
FWDpin = 0;
BWDpin = 0;
//Initierar mätning av hjulencoders
//Interrupt för att mäta PPS
//tick.attach(callback(this, &motor::pulsePerSecond), interval);
runnable = true;
}
void motor::runBasic(float speed){
if(runnable == true){
//Sätter riktning på motor
FWDpin = (speed > 0);
BWDpin = (speed < 0);
//Sätter duty-cycle på motor
motorPWM.write(fabs(speed));
}
}
//Stop-funktion
void motor::stop(){
FWDpin = 0;
BWDpin = 0;
motorPWM.write(0);
}
/******************
* Linje.h *
******************\
#include "mbed.h"
#include <motorstyrning.h>
#define right 1
#define down 2
#define up 3
#define left 4
#define rightTurn 'R'
#define leftTurn 'L'
#define turn180 'B'
#define forward 'F'
#define fast 0.7
#define medium 0.65
#define slow 0.6
// Datatyp för bilens all bilens information
class carData {
public:
carData(int xStart, int yStart, int hStart);
int x; // X-Koordinat för bilen
int y; // Y-Koordinat för bilen
int h; // Körriktning för bilen
const int minUpdateIntervall = 500; // Minsta acceptabla tid passerad sedan föregående koordinat
const int xbounds[2] = {0, 16}; // Yttre gränser X-led
const int ybounds[2] = {0, 8}; // Yttre gränser Y-led
//Initiera i constructor
bool outofBounds; // Flagga för om bilen uppmätt linje utanför rutnät
char state;
Timer updateIntervallTimer; // Timer för senast uppdaterad koordinat uppmätt
bool updateIntervalOK(); // Kollar om koordinaterna inte nyligen är uppdaterade
bool changeableInDirection(); // Kollar om koordinaten går att ändra i körriktning
void resetUpdateIntervallTimer(); // Nollställer updateIntervallTimer
void update(); // Uppdatera koordinat
private:
};
// Klass för yttre IR-sensor
class yttreIR {
public:
yttreIR(PinName pin, carData* pD1);
carData* adress; //Adress till dataklass carData
const int minOnIntervall = 15; // Minnsta acceptabla tiden sensor ska var över linje
bool onLine; // Sensor över linje
float timeOnLine; // Uppmätt tid sensor är över linje
Timer timer; //Timer för sensor över linje
bool Turn; // Kollar vilket håll AGV ska svänga åt
void startIrTimer(); // Startar timer för IR-sensor
void stopIrTimer(); // Koordinat-räknaren
private:
InterruptIn sensor;
};
class sensorRamp {
public:
sensorRamp(carData* B1);
carData* adress;
void Error();
void motorPIDcontrolSlow(float &leftMotorSpeedS, float &rightMotorSpeedS,float error);
void motorPIDcontrolFast(float &leftMotorSpeedF, float &rightMotorSpeedF,float error);
void calculatePID(float error);
void Ramp();
bool turn(int degree, motor* adressR, motor* adressL, bool Lturn, bool Rturn);
//3v som Vth
/***************
* MED RAMP *
**************** */
//float Kp = 0.02;
//float Kd = 0.08;
float liteFel = 1;
float merFel = 1.5;
Timer fel;
/***************
* FÖR SVÄNG *
*****************/
Timer timTurn;
const int turnTimeR = 100;
const int turnTimeL = 200;
/***************
* MED KAMERA *
**************** */
float Kp = 0.1;
float Kd = 0.02;
private:
int turnPulse;
float P,D;
int counter;
float PIDvalue;
float baseSpeedRSlow = 0.7865;
float baseSpeedLSlow = 0.8;
float basespeedRFast = 0.8765;
float basespeedLFast = 0.89;
//R = 0.7865
//L = 0.8
//R snabb, ganska stabil = 0.8365
//L snabb, ganska stabil = 0.85
float error,previousError;
bool felR;
bool felL;
DigitalIn IR_LL{PB_9};
DigitalIn IR_RR{PB_5};
DigitalIn IR_L{PB_8};
DigitalIn IR_R{PB_6};
DigitalIn IR_C{PB_7};
};
/******************
* Linje.cpp *
******************\
#include <linje.h>
/*************************
* CAR DATA *
************************ */
// Konstruktor för klass för bil, carData
carData::carData(int xStart, int yStart, int hStart){
x = xStart; // X-Koordinat för bilen
y = yStart; // Y-Koordinat för bilen
h = hStart; // Körriktning för bilen
outofBounds = false;
}
bool carData::updateIntervalOK(){
if(updateIntervallTimer.read_ms() > minUpdateIntervall && state != 'T') return true;
return false;
}
bool carData::changeableInDirection(){
if((h == right && x < xbounds[1]) || (h == left && x > xbounds[0])) return true; // Change in direction of X
else if((h == down && y > ybounds[0]) || (h == up && y < ybounds[1])) return true; // Change in direction of Y
else{
outofBounds = true;
return false;
}
}
void carData::update(){
if(h == right) x++;
else if (h == down) y--;
else if (h == up) y++;
else if (h == left) x--;
resetUpdateIntervallTimer();
}
void carData::resetUpdateIntervallTimer(){
//Resets timer to 0, continue counting
updateIntervallTimer.reset();
}
/*************************
* YTTRE IR *
************************ */
// Konstruktor för klassen yttre IR-diod, yttreIR
yttreIR::yttreIR(PinName pin, carData* pD1) : sensor(pin){
// Interrupt på när sensor upp och ned från linje
sensor.fall(callback(this, &yttreIR::startIrTimer));
sensor.rise(callback(this, &yttreIR::stopIrTimer));
adress = pD1; //Adress till dataklass carData
//Const i header.
timeOnLine = 0;
Turn = false; //Bool för att kolla vilket håll vi ska svänga åt
}
void yttreIR::startIrTimer(){
timer.start();
onLine = true;
}
void yttreIR::stopIrTimer(){
timer.stop();
timeOnLine = timer.read_ms();
timer.reset();
onLine = false;
if( adress->updateIntervalOK() && (timeOnLine > minOnIntervall) ){
if(adress->changeableInDirection()) adress->update();
timeOnLine = 0;
Turn = true;
}
}
/*************************
* SENSORRAMP *
************************ */
sensorRamp::sensorRamp(carData* B1) {
adress = B1;
P = 0;
D = 0;
PIDvalue = 0;
previousError = 0;
error = 0;
counter = 0;
}
void sensorRamp::Error()
{ //OM C är hög
if(IR_L == 1 && IR_C == 0 && IR_R == 1) {
error = 0;
}
else if(IR_L == 1 && IR_C == 0 && IR_R == 1 && felR){
fel.start();
error = -liteFel;
}
else if(IR_L == 1 && IR_C == 0 && IR_R == 1 && felL){
fel.start();
error = liteFel;
}
else if(IR_L == 0 && IR_C == 0 && IR_R == 1 ){//Om C samt L är låga
error = liteFel;
}
else if (IR_L == 0 && IR_C == 1 && IR_R == 1) {
error = merFel;
felR = true;
}
else if (IR_L == 1 && IR_C == 0 && IR_R == 0) {// Om C samt R är låga
error = -liteFel;
}
else if (IR_L == 1 && IR_C == 1 && IR_R == 0) {
error = -merFel;
felL = true;
}
if(fel.read_ms() > 50 ){
felR = false;
felL = false;
fel.stop();
fel.reset();
}
}
void sensorRamp::calculatePID(float error) {
//Error();
P = error;
D = error-previousError;
PIDvalue = (Kp*P) + (Kd*D);
previousError = error;
}
//Funktion för att köra beräkna en hastighet som är långsam
void sensorRamp::motorPIDcontrolSlow(float &leftMotorSpeedS, float &rightMotorSpeedS, float error) {
calculatePID(error);
leftMotorSpeedS = baseSpeedLSlow - PIDvalue;
rightMotorSpeedS = baseSpeedRSlow + PIDvalue;
if(leftMotorSpeedS > 0.9) {
leftMotorSpeedS = 0.9;
}
if(rightMotorSpeedS > 0.9){
rightMotorSpeedS = 0.9;
}
}
//Funktion för att köra beräkna en hastighet som är snabb
void sensorRamp::motorPIDcontrolFast(float &leftMotorSpeedF, float &rightMotorSpeedF, float error) {
calculatePID(error);
leftMotorSpeedF = basespeedLFast - PIDvalue;
rightMotorSpeedF = basespeedRFast + PIDvalue;
if(leftMotorSpeedF > 0.9) {
leftMotorSpeedF = 0.9;
}
if(rightMotorSpeedF > 0.9){
rightMotorSpeedF = 0.9;
}
}
bool sensorRamp::turn(int degree, motor* adressR, motor* adressL, bool Lturn, bool Rturn){
if(degree == leftTurn){
timTurn.start();
//adressL->counter_pulses = 0;
adress->state = 'T';
adressR->runBasic(fast);
adressL->runBasic(-fast);
timTurn.reset();
//Sväng tills timer når 100 ms
while(timTurn.read_ms() < 100){}
timTurn.stop();
//Sväng tills LL blir låg(På tejp)
while(IR_LL){}
adressR->runBasic(medium);
adressL->runBasic(-medium);
//Sväng tills L blir låg(På tejp)
while(IR_L){}
//Sakta ner
adressR->runBasic(slow);
adressL->runBasic(-slow);
//Sväng tills C blir låg och stanna
while(IR_C){}
adressR->stop();
adressL->stop();
return true;
}
else if (degree == rightTurn) {
timTurn.start();
adress->state = 'T';
//adressL->counter_pulses = 0;
adressR->runBasic(-fast);
adressL->runBasic(fast);
timTurn.reset();
while(timTurn.read_ms() < 100){}
timTurn.stop();
while(IR_RR){}
adressR->runBasic(-fast);
adressL->runBasic(fast);
while(IR_R){}
adressR->runBasic(-slow);
adressL->runBasic(slow);
while(IR_C){}
adressR->stop();
adressL->stop();
return true;
}
else if (degree == turn180) {
//Om de båda yttre sensorerna har räknat
//Spelar det ingen roll hur AGV svänger
if(Rturn == true && Lturn == true){
adress->state = 'T';
//Sväng höger
timTurn.start();
adressR->runBasic(-fast);
adressL->runBasic(fast);
while(timTurn.read_ms() < turnTimeR){}
while(IR_RR){}
adressR->runBasic(-fast);
adressL->runBasic(fast);
timTurn.reset();
while(timTurn.read_ms() < turnTimeR){}
timTurn.stop();
timTurn.reset();
while(IR_RR){}
adressR->runBasic(-medium);
adressL->runBasic(medium);
while(IR_R){}
adressR->runBasic(-slow);
adressL->runBasic(slow);
while(IR_C){}
adressL->stop();
adressR->stop();
}
//Endash RR har räknat kordinat
//Sväng åt höger
else if(Rturn == true && Lturn == false){
adress->state = 'T';
//Sväng höger
timTurn.start();
adressR->runBasic(-fast);
adressL->runBasic(fast);
while(timTurn.read_ms() < turnTimeR){}
while(IR_RR){}
adressR->runBasic(-fast);
adressL->runBasic(fast);
timTurn.reset();
while(timTurn.read_ms() < turnTimeR){}
timTurn.stop();
timTurn.reset();
while(IR_RR){}
adressR->runBasic(-medium);
adressL->runBasic(medium);
while(IR_R){}
adressR->runBasic(-slow);
adressL->runBasic(slow);
while(IR_C){}
adressL->stop();
adressR->stop();
}
//Endash LL har räknat kordinat
//Sväng åt vänster
else if (Rturn == false && Lturn == true){
adress->state = 'T';
timTurn.start();
adressR->runBasic(fast);
adressL->runBasic(-fast);
while(timTurn.read_ms() < turnTimeL){}
while(IR_LL){}
adressR->runBasic(fast);
adressL->runBasic(-fast);
timTurn.reset();
while(timTurn.read_ms() < turnTimeL){}
timTurn.stop();
timTurn.reset();
while(IR_LL){}
adressR->runBasic(slow);
adressL->runBasic(-slow);
while(IR_L){}
adressR->stop();
adressL->stop();
}
return true;
}
else if (degree == forward){
return true;
}
else{
printf("%i is not at correct degree\n", degree);
return false;
}
}
/******************
* bas.h *
******************\
#include "mbed.h"
#include "LED.h"
int getHeading(int h, char turn);
int charToInt(char A);
/******************
* bas.cpp *
******************\
#include "Bas.h"
//samling av godtyckliga funktioner som inte tillhör någon klass
int getHeading(int h, char turn)
{
if(h == 1){
if(turn == 'R'){
h = 2;
}
else if (turn == 'L'){
h = 3;
}
else if (turn == 'B'){
h = 4;
}
else if(turn == 'F')
{
h = 1;
}
}
else if(h == 2)
{
if(turn == 'R'){
h = 4;
}
else if (turn == 'L'){
h = 1;
}
else if (turn == 'B'){
h = 3;
}
else if(turn == 'F')
{
h = 2;
}
}
else if (h == 3) {
if(turn == 'R'){
h = 1;
}
else if (turn == 'L'){
h = 4;
}
else if (turn == 'B'){
h = 2;
}
else if(turn == 'F')
{
h = 3;
}
}
else if(h == 4){
if(turn == 'R'){
h = 3;
}
else if (turn == 'L'){
h = 2;
}
else if (turn == 'B'){
h = 1;
}
else if(turn == 'F')
{
h = 4;
}
}
return h;
}
//Gör om chars till Ints
int charToInt(char A)
{
int kord = 0;
if(A >= 'a')
{
kord = A - 'W';
}
else
{
kord = A - '0';
}
return kord;
}
Please register or sign in to comment