\********************
* main.cpp *
********************\
#include "mbed.h"
#include "Bas.h"
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); //Kommunikation med kamera
carData D1(0,0,1); // Skapar objekt för bilens data
carData* pD1 = &D1; // med start o (0,0) och riktning höger(x++)
D1.updateIntervallTimer.start(); // statart timer för senast uppdaterad koordinat
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); //BT-kommunikation
yttreIR LL{PC_8, pD1}; //Räknare
yttreIR RR{PB_5, pD1}; //Räknare
IRcenter C{PB_7}; //Räknare för Extra körning
IRcenter* PC = &C; //Pekare till klassen IRCenter
C.updateOk.start(); //Startar timer för räkning av koordinater
sensorRamp Ramp(&D1); //Sväng och PID
LED lampa(pD1); //Status-led
Timer stopTime; //Timer för att plocka rätt tid
DigitalIn DIP1(PA_4);
char list[100][5]; //Koordinatlista
//Sätt hela list till '\0'
memset(list, '\0', sizeof(list));
float SpeedL = 0; //Motorhastighet
float SpeedR = 0; //Motorhastighet
int xTarget = 0; //Target för x-koord, läst från list[]
int yTarget = 0; //Target för y-koord, läst från list[]
int cTarget =0; //Target för räknare vid diagonalkörning.
char turn; //Vilket håll AGV ska svänga åt i nod
char nodeType; //Typ av nod
char uppSvong;
int listLoc = 0; //Plats i koordinatlista
int targetC = 0;
float old_data; //För att endast reglera med ny data
float data; //För att endast reglera med ny data
/*****************************
* 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,%c \n", list[i][0],list[i][1],list[i][2],list[i][3],list[i][4]);
}
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
/****************************
* BAS *
*****************************/
if(!DIP1){
while (true) {
//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){
//Hämta data från kamera.
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.motorPIDcontrol(SpeedL, SpeedR, data, true);
}
else if ((D1.h == 2 || D1.h == 3 ) && (abs(D1.y - yTarget) >= 2)) {
Ramp.motorPIDcontrol(SpeedL, SpeedR, data, true);
}
//Köra långsamt för att svängen ska bli bra
else {
Ramp.motorPIDcontrol(SpeedL, SpeedR, data, false);
}
}
//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.
if(nodeType == 's'){
//Timer för att stanna i 3 sek.
stopTime.start();
L.stop();
R.stop();
//Plockar
D1.state = 'P';
while(true){
if(stopTime.read() > 3){
stopTime.stop();
stopTime.reset();
printf("Färdig med plock \n");
break;
}
}
}
//Återställer sväng-bool.
LL.Turn = false;
RR.Turn = false;
//AGV kör
D1.state = 'R';
//Inkrementera list index
listLoc++;
//Kollar om slutet av listan är nådd
if(list[listLoc][0] == '\0'){
D1.state = 'm';
BT.endStatus();
printf("Körorder klar!");
L.stop();
R.stop();
return 0;
}
}
}
/****************************
* EXTRA *
*****************************/
int target[2] = {};
int lastTarget[2] = {0,0};
int xyDiff[2] = {};
if(DIP1){
while(true){
//Läser in data för nästa nod
target[0] = charToInt(list[listLoc][0]);
target[1] = charToInt(list[listLoc][1]);
nodeType = list[listLoc][2];
turn = list[listLoc][3];
uppSvong = list[listLoc][4];
nodeDiff(target, lastTarget, xyDiff, pD1);
//Följ line som vanligt om sant.
if(xyDiff[0] == 0 || xyDiff[1] == 0){
//För att bara åka framåt.
kamera.send('F');
D1.countEnable = true;
C.enableC = false;
}
else{
//Skicka data för nästa nod.
kamera.nextNode(xyDiff, nodeType);
D1.countEnable = false;
C.enableC = true;
targetC = getCtarget(D1.x, D1.y,target[0], target[1]);
}
//SVÄNG TILLS 'S' MOTTAGET
while(!kamera.nodeAim){
//"svängläge"
data = kamera.data();
Ramp.turnToCord(data,&R,&L);
}
while(targetC != C.cCounter){
//KÖR TILL NÄSTA NOD
data = kamera.data();
if(data != old_data){
old_data = data;
Ramp.motorPIDcontrol(SpeedL, SpeedR, data, false);
//Skriv den uträknade hastigheten till motorerna
R.runBasic(SpeedR);
L.runBasic(SpeedL);
}
}
C.CounterReset();
//NÄR AGV ÄR FRAMME VID NÄSTA NOD
kamera.send('N');
//Svänger upp till en linje och uppdaterar AGVS position
Ramp.turnToLine(uppSvong, target[0], target[1], &R, &L);
//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.
if(nodeType == 's' || nodeType == 'p'){
//Timer för att stanna i 3 sek.
stopTime.start();
L.stop();
R.stop();
//Plockar
D1.state = 'P';
while(true){
if(stopTime.read() > 3){
stopTime.stop();
stopTime.reset();
printf("Färdig med plock \n");
break;
}
}
}
//Återställer sväng-bool.
LL.Turn = false;
RR.Turn = false;
//Inkrementera list index
listLoc++;
//Kollar om slutet av listan är nådd
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[][5]); //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 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
};
//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
void nextNode(int target[2], const char &nodeType); //Skickar nästa nod
bool nodeAim; //True när kameran siktat in sig på nod.
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
bool ended;
};
\********************
* 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;
}
void HC05::startStatus(){
//Timerinterrupt för att skicka status
status.attach(callback(this, &HC05::sendStatus), 0.5);
}
//Startar serial interrupt för stop
void HC05::StartStopInterupt(){
BT.attach(callback(this,&HC05::serialStop),Serial::RxIrq);
}
void HC05::serialStop()
{
if(BT.readable()){
if(BT.getc() == 'X'){
cData->state = 'Q'; //Felkod
adrL->stop(); //Stanna motor
adrR->stop(); //Stanna motor
adrL->runnable = false; //Låser motor
adrR->runnable = false; //Låser motor
}
}
}
void HC05::endStatus(){
status.detach();
}
//Funktion för att ta emot koordinater från ÖS
bool HC05::getCoord(char list[][5]) {
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 < 5) {
//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';
}
}
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 == 'S'){
nodeAim = true;
}
else 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);
}
//Skicka nästa nod.
void camCom::nextNode(int target[2], const char &nodeType){
if(cam.writeable()){
if(target[0] >= 10){
cam.putc(target[0] + 'W');
}
else{
cam.putc(target[0] + '0');
}
if(target[1] >= 10){
cam.putc(target[1] + 'W');
}
else{
cam.putc(target[1] + '0');
}
cam.putc(nodeType);
}
return;
}
\********************
* 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
#define turnspeed 0.64
// 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
//Initiera i constructor
bool outofBounds; // Flagga för om bilen uppmätt linje utanför rutnät
char state;
bool countEnable;
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
void updateLocation(int _X,int _Y);
private:
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
};
// Klass för yttre IR-sensor
class yttreIR {
public:
yttreIR(PinName pin, carData* pD1);
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:
carData* data; //Adress till dataklass carData
const int minOnIntervall = 15; // Minnsta acceptabla tiden sensor ska var över linje
float timeOnLine; // Uppmätt tid sensor är över linje
InterruptIn sensor; //Interrupt för sensor
Timer timer; //Timer för sensor över linje
};
class IRcenter {
public:
IRcenter(PinName pin);
void mittenIrstart();
void mittenIrStop();
void CounterReset();
int cCounter;
bool enableC;
Timer updateOk;
private:
carData* data;
const int minOnIntervallC = 15;
float timeOnLineC;
const int minTimeUpdate = 100; // Hur ofta den får räkna linjer
InterruptIn sensor;
Timer timeLine;
};
class sensorRamp {
public:
sensorRamp(carData* B1);
void motorPIDcontrol(float &leftMotorSpeed, float &rightMotorSpeed,
float error, const bool hast);
void calculatePID(float error);
bool turn(int degree, motor* adressR, motor* adressL, bool Lturn, bool Rturn);
void turnToLine(char turn, int xNext, int yNext, motor* adressR, motor* adressL);//Funkton för att snurra på platsen tills krysset är hittat
void turnToCord(float data, motor* adressR, motor* adressL);
private:
/***************
* FÖR SVÄNG *
*****************/
Timer timTurn;
const int turnTimeR = 100;
const int turnTimeL = 200;
/***************
* MED KAMERA *
**************** */
float Kp = 0.1;
float Kd = 0.02;
carData* data;
//PID
float P,D;
float PIDvalue;
float error,previousError;
double k; //Lutning för linje mellan två noder,
int tempH;
//KAN TRIMMAS MER
float baseSpeedRSlow = 0.7865;
float baseSpeedLSlow = 0.8;
float basespeedRFast = 0.8765;
float basespeedLFast = 0.89;
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;
countEnable = true;
}
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(countEnable){
if(h == right) x++;
else if (h == down) y--;
else if (h == up) y++;
else if (h == left) x--;
resetUpdateIntervallTimer();
}
}
void carData::updateLocation(int _X, int _Y){
x = _X;
y = _Y;
}
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));
data = 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();
}
void yttreIR::stopIrTimer(){
timer.stop();
timeOnLine = timer.read_ms();
timer.reset();
if( data->updateIntervalOK() && (timeOnLine > minOnIntervall) ){
if(data->changeableInDirection()) {
data->update();
}
timeOnLine = 0;
Turn = true;
}
}
/*************************
* Center IR *
************************ */
IRcenter::IRcenter(PinName pin)
:sensor(pin){
sensor.fall(callback(this,&IRcenter::mittenIrstart));
sensor.rise(callback(this,&IRcenter::mittenIrStop));
cCounter = 0;
enableC = false;
}
void IRcenter::mittenIrstart(){
timeLine.start();
}
void IRcenter::mittenIrStop(){
timeLine.stop();
timeOnLineC = timeLine.read();
timeLine.reset();
if(timeOnLineC > minOnIntervallC && enableC == true && updateOk.read() > minTimeUpdate){
cCounter++;
updateOk.reset();
}
}
void IRcenter::CounterReset(){
cCounter = 0;
}
/*************************
* SENSORRAMP *
************************ */
sensorRamp::sensorRamp(carData* B1) {
data = B1;
P = 0;
D = 0;
PIDvalue = 0;
previousError = 0;
error = 0;
}
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::motorPIDcontrol(float &leftMotorSpeed, float &rightMotorSpeed,
float error, const bool hast) {
calculatePID(error);
//Kör snabbt om hast = true
if(hast){
leftMotorSpeed = basespeedLFast - PIDvalue;
rightMotorSpeed = basespeedRFast + PIDvalue;
}
//Långsamt
else{
leftMotorSpeed = baseSpeedLSlow - PIDvalue;
rightMotorSpeed = baseSpeedRSlow + PIDvalue;
}
if(leftMotorSpeed > 0.9) {
leftMotorSpeed = 0.9;
}
if(rightMotorSpeed > 0.9){
rightMotorSpeed = 0.9;
}
}
bool sensorRamp::turn(int degree, motor* adressR, motor* adressL, bool Lturn, bool Rturn){
if(degree == leftTurn){
timTurn.start();
data->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();
data->state = 'T';
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){
data->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){
data->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){
data->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;
}
}
void sensorRamp::turnToLine(char turn,int xNext,int yNext, motor* adressR, motor* adressL){
k = (yNext-data->y)/(xNext-data->x);
if(turn == 'L'){
//Sväng upp åt vänster relativt AGV
adressL->runBasic(-medium);
adressR->runBasic(medium);
while(IR_L){}
adressL->stop();
adressR->stop();
}
else if(turn == 'R'){
//Svänger upp åt höger relativt AGV
adressL->runBasic(medium);
adressR->runBasic(-medium);
while(IR_R){}
adressL->stop();
adressR->stop();
}
if(data->h == 4){
if(k > 0 && turn == 'L'){data->h = 2;}
else if(k < 0 && turn == 'R'){data->h = 3;}
}
else if(data->h == 2){
if(k > 0 && turn == 'R'){data->h = 4;}
else if(k < 0 && turn == 'L'){data->h = 1;}
}
else if(data->h == 3){
if(k > 0 && turn == 'R'){data->h = 1;}
else if(k < 0 && turn == 'L'){data->h = 4;}
}
else if(data->h == 1){
if(k > 0 && turn == 'L'){data->h = 3;}
else if(k < 0 && turn == 'R'){data->h = 2;}
}
//Uppdaterar AGVs koordinater
data->x = xNext;
data->y = yNext;
}
void sensorRamp::turnToCord(float data,motor* adressR,motor* adressL)
{
if(data > 0){
adressL->runBasic(0.62);
adressR->runBasic(-0.62);
}
else if (data < 0) {
adressL->runBasic(-0.62);
adressR->runBasic(0.62);
}
}
/************************
* motorstyrning.h *
************************/
#include "mbed.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
};
/************************
* 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;
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);
}
/************************
* LED.h *
************************/
#include"mbed.h"
#include"kommunikation.h"
#include"cmath"
class LED {
public:
LED(carData* pD1); //Constructor
void lys(); //Funktion för att lysa LED, kallas på av timer-interrupt
private:
PwmOut pwmLED; //PWM-signal
carData* data; //För att få info om AGV
int count; //Räknare
int countV; //Räknare för "vänta"
bool firstP; //Första iterationen av "plock"
bool firstQ; //Första iterationen av "fel"
bool firstV; //Första iterationen av "vänta"
bool countDown; //Används för "plock"-animation
Ticker blink; //Timer interrupt
};
/************************
* LED.cpp *
************************/
#include"LED.h"
LED::LED(carData *pD1) : pwmLED(PA_0){
pwmLED = 1.0;
pwmLED.period_ms(1);
data = pD1;
//Led-animationer görs via timer-interrupt 33 ggr/sek
blink.attach(callback(this, &LED::lys), 0.03);
}
void LED::lys(){
//När AGV "plockar", LED andas
if(data->state == 'P'){
//Återställer count till 0 på rätt sätlle
if(firstP){
firstQ = true;
firstP = false;
count = 0;
countDown = false;
}
//Öka för att minska lägsta värde, Count == 30 ~ helt släckt
if(count == 25 && !countDown) countDown = true;
//Öka för att minska högsta värde, Count == 0 är fullt ös
if(count == 0 && countDown) countDown = false;
//Funktion för att få "andnings"-animation
if(!countDown){
pwmLED = pow(10,((count+5)*0.01)) -1;
count++;
}
if(countDown){
pwmLED = pow(10,((count+5)*0.01)) -1;
count--;
}
}
//När AGV är i ett fel-state, helt vanligt blinkande
if(data->state == 'Q' || data->state == 'q'){
if(firstQ){
firstQ = false;
firstP = true;
firstV = true;
count = 3;
}
if(count == 3){
pwmLED = 0.0;
}
if(count == 5){
pwmLED = 1.0;
count = 0;
}
count++;
}
//När AGV kör, Lyser vanligt. Högre duty cycle ger mindre ljus.
if(data->state == 'R' || data->state == 'T'){
if(firstQ){
firstQ = true;
firstP = true;
firstV = true;
}
pwmLED = 0.7;
}
//När AGV väntar på koordinater och startkommando. Snabba blink -> vänta.
if(data->state == 'V'){
if(firstV){
firstQ = true;
firstP = true;
firstV = false;
count = 3;
countV = 0;
}
if(countV > 1 ) {
countV++;
}
else if(count == 7){
pwmLED = 0.0;
}
else if(count == 9){
pwmLED = 1.0;
count = 0;
countV++;
}
if(countV == 30){
countV = 0;
count = 0;
}
count++;
}
//När AGV är färdig med hela rutten
if(data->state == 'm'){
pwmLED = 0.0;
}
}
/************************
* BAS.h *
************************/
#include "mbed.h"
#include "LED.h"
int getHeading(int h, char turn);
int charToInt(char A);
void nodeDiff(int targ[2], int lastTarg[2], int xy_d[2], carData* data);
int getCtarget(int xNow, int yNow, int xNext, int yNext);
/************************
* 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;
}
void nodeDiff(int targ[2], int lastTarg[2], int xy_d[2], carData* data){
if(data->h == 1){
xy_d[0] = -(targ[1]-lastTarg[1]);
xy_d[1] = targ[0]-lastTarg[0];
}
else if(data->h == 2){
xy_d[0] = -(targ[0]-lastTarg[0]);
xy_d[1] = -(targ[1]-lastTarg[1]);
}
else if(data->h == 3){
xy_d[0] = targ[0]-lastTarg[0];
xy_d[1] = targ[1]-lastTarg[1];
}
else if(data->h == 4){
xy_d[0] = targ[1]-lastTarg[1];
xy_d[1] = -(targ[0]-lastTarg[0]);
}
lastTarg[0] = targ[0];
lastTarg[1] = targ[0];
}
int getCtarget(int xNow, int yNow, int xNext, int yNext)
{
int dX = xNext - xNow;
int dY = yNext - yNow;
int n = abs(dX-dY);
int cCounterTarget = 0;
switch(n){
case 0:
cCounterTarget = dX;
break;
case 2:
cCounterTarget = 3;
break;
case 3:
cCounterTarget = 4;
break;
}
if((dX == 3 && dY == 2)|| (dX == 2 && dY == 3)){
cCounterTarget = 4;
}
else if((dX == 2 && dY == 1)||(dX = 1 && dY == 2)){
cCounterTarget = 2;
}
return cCounterTarget;
}