271 lines
6.4 KiB
C
271 lines
6.4 KiB
C
//#
|
|
//# main.c
|
|
//#
|
|
//# =======================================================
|
|
//# Lab i realtidssystem
|
|
//# Styrning av tåg
|
|
//#
|
|
//#
|
|
//# Christian Ohlsson, Daniel Alfredsson
|
|
//# Karlstads universitet
|
|
//# 991008
|
|
//# =======================================================
|
|
|
|
#include <stdio.h>
|
|
#include <personal/mon.h>
|
|
#include <red/r_thread.h>
|
|
#include <blue/b_thread.h>
|
|
#include "taglab_r.h"
|
|
#include "train.h"
|
|
#include "hal.h"
|
|
|
|
//#
|
|
//# SPEED
|
|
//# Här anger vi standard hastighet på tågen.
|
|
#define SPEED 12
|
|
|
|
//#
|
|
//# bsPosixTime_t
|
|
//# Anger Rubustid.
|
|
static bsPosixTime_t const timeSec = {90L, 0L};
|
|
|
|
//#
|
|
//# blueSleep()
|
|
//# Berättar för kompilatorn att denna
|
|
//# externa funktion används.
|
|
extern void blueSleep();
|
|
|
|
//#
|
|
//# Train_t
|
|
//# Skapar våra två tåg. Dessa är globala.
|
|
Train_t Train0;
|
|
Train_t Train8;
|
|
|
|
//#
|
|
//# sleep()
|
|
//# Låter Rubus sova i ett antal millisekunder.
|
|
void sleep(unsigned int millisecs) {
|
|
TYPE_TIME tp;
|
|
bsPosixTime_t t;
|
|
t.tvSec = 0;
|
|
t.tvNsec = millisecs * 1000000L;
|
|
tp = bsTvToJiffies((bsPosixTime_t*)&t);
|
|
blueSleep(&tp);
|
|
}
|
|
|
|
//#
|
|
//# blueError()
|
|
//# Anropas om fel i blå kärnan inträffar.
|
|
INT blueError(INT code, blueThreadId_t thread) {
|
|
INT error = code;
|
|
blueThreadId_t id = thread;
|
|
halWrite("Det blev blått fel...");
|
|
halHalt();
|
|
return(RCODE_OK);
|
|
}
|
|
|
|
//#
|
|
//# redError()
|
|
//# Anropas om fel i röda kärnan inträffar.
|
|
INT redError(INT code, VOIDP thread) {
|
|
halWrite("Det blev rött fel...");
|
|
sleep(300);
|
|
halExit();
|
|
return(RCODE_OK);
|
|
}
|
|
|
|
//#
|
|
//# train_init()
|
|
//# Initierar tågbibliotek, skapar tåg,
|
|
//# ställer växlar och kör igång tågen.
|
|
bool_t train_init() {
|
|
if(trainlib_init() != True) {
|
|
halWrite("Failed to initialized library.\n");
|
|
return False;
|
|
}
|
|
else {
|
|
halWrite("Library initialized.\n"); sleep(200);
|
|
trainlib_start(); sleep(100);
|
|
Train0 = train_create(60); sleep(100);
|
|
Train8 = train_create(78); sleep(100);
|
|
switch_open(1); sleep(100);
|
|
switch_open(4); sleep(100);
|
|
switch_close(2); sleep(100);
|
|
switch_close(3); sleep(100);
|
|
train_set_speed(Train8, SPEED); sleep(100);
|
|
train_set_speed(Train0, SPEED); sleep(100);
|
|
return True;
|
|
}
|
|
}
|
|
|
|
//#
|
|
//# blueIdleMain()
|
|
//# Körs om ingen annan process körs.
|
|
void blueIdleMain(void) {
|
|
halWrite("\nblueIdleMain()");
|
|
while(1);
|
|
}
|
|
|
|
//#
|
|
//# blueFunk()
|
|
//# Styr hela förloppet, dvs ställer om
|
|
//# växlar och startar och stoppar tåg.
|
|
void blueFunk() {
|
|
int go=0;
|
|
uint mod1, mod2;
|
|
if(!train_init()) halHalt();
|
|
while(1) {
|
|
// sleep(5);
|
|
mod1 = sensor_read(1);
|
|
// sleep(10);
|
|
mod2 = sensor_read(2);
|
|
// if(mod1 != 0) printf("\nmod1: %d",mod1);
|
|
// if(mod2 != 0) printf("\nmod2: %d", mod2);
|
|
|
|
// Begin switch (Behövs detta?)
|
|
if(mod2&0x0080 || mod2&0x8000) {
|
|
switch_open(2); // 0:an kan köra
|
|
sleep(100);
|
|
}
|
|
|
|
if(mod2&0x0040 || mod2&0x4000) {
|
|
switch_close(2); // 8:an kan köra
|
|
sleep(100);
|
|
}
|
|
|
|
// Hastighetsändringar för tåget
|
|
if (mod1 & 0x0020 || mod2 & 0x0001) { //8 kommer till krysset
|
|
train_set_speed(Train8, 0);
|
|
sleep(100);
|
|
switch_open(2); //från vänster
|
|
go=1;
|
|
sleep(100);
|
|
}
|
|
if (mod2 & 0x8000 || mod1 & 0x0010) { //8 kommer till krysset
|
|
train_set_speed(Train8, 0);
|
|
sleep(100);
|
|
switch_open(2); //från höger
|
|
go=2;
|
|
sleep(100);
|
|
}
|
|
if (go == 1) { //Tåget väntar i krysset
|
|
if (mod2 & 0x0040 || mod2 & 0x4000 || mod1 & 0x0080 ||
|
|
mod1 & 0x4000 || mod1 & 0x8000 || mod1 & 0x0001 ) {
|
|
switch_close(2);
|
|
sleep(100);
|
|
train_set_speed(Train8, SPEED);
|
|
go=0;
|
|
sleep(100);
|
|
}
|
|
}
|
|
if (go == 2) { //Tåget väntar i krysset
|
|
if (mod1 & 0x0001 ||mod1 & 0x0002 || mod1 & 0x0008 ||
|
|
mod2 & 0x0004 || mod2 & 0x0008 || mod2 & 0x0010 ) {
|
|
switch_open(2);
|
|
sleep(100);
|
|
train_set_speed(Train8, SPEED);
|
|
go=0;
|
|
sleep(100);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
//#
|
|
//# dummy()
|
|
//# Den röda dummytrådens funktion.
|
|
void dummy() {
|
|
halWrite("\nDummy thread running...");
|
|
}
|
|
|
|
//#
|
|
//# main()
|
|
//# Initierar Rubus och avbryter om
|
|
//# om något går fel.
|
|
void main(void) {
|
|
halWrite("\nProgrammet startades!\n");
|
|
|
|
if (halInit() != RCODE_OK)
|
|
halHalt();
|
|
if (bsInit(OBJECT_BASIC_QUEUE | _RO_RESET) != RCODE_OK)
|
|
halHalt();
|
|
if (redInit(&redSchedule) != RCODE_OK)
|
|
halHalt();
|
|
if (blueInit(OBJECT_BLUE_MUTEX | OBJECT_BLUE_MSG | OBJECT_BLUE_THREAD | _RO_RESET) != RCODE_OK)
|
|
halHalt();
|
|
|
|
halBsTimerStart();
|
|
blueStart();
|
|
}
|
|
|
|
/*
|
|
GAMMAL KOD
|
|
|
|
TYPE_TIME startTime, stopTime, timePeriod;
|
|
timePeriod = bsTvToJiffies((bsPosixTime_t *)&timeSec);
|
|
|
|
printf("start");
|
|
if(!train_init()) halHalt();
|
|
printf("stopp");
|
|
bsClockGetTime(&startTime);
|
|
|
|
|
|
while(1) {printf("hey");};
|
|
stopTrain();
|
|
halWrite("\nProgrammet avslutades...");
|
|
halExit();
|
|
|
|
void readSensor(uint mod1, uint mod2) {
|
|
halWrite("\nREAD_SENSOR");
|
|
mod1 = sensor_read(1);
|
|
sleep(100);
|
|
mod2 = sensor_read(2);
|
|
|
|
if(mod1 & 1) { halWrite("\n mod:1 & __1__\n"); }
|
|
else if(mod1 & 2) { halWrite("\n mod:1 & __2__\n"); }
|
|
else if(mod1 & 3) { halWrite("\n mod:1 & __3__\n"); }
|
|
else if(mod1 & 4) { halWrite("\n mod:1 & __4__\n"); }
|
|
else if(mod1 & 5) { halWrite("\n mod:1 & __5__\n"); }
|
|
else if(mod1 & 6) { halWrite("\n mod:1 & __6__\n"); }
|
|
else if(mod1 & 7) { halWrite("\n mod:1 & __7__\n"); }
|
|
else if(mod1 & 8) { halWrite("\n mod:1 & __8__\n"); }
|
|
else if(mod1 & 9) { halWrite("\n mod:1 & __9__\n"); }
|
|
else if(mod1 & 10) { halWrite("\n mod:1 & __10_\n"); }
|
|
else if(mod2 & 1) { halWrite("\n mod:2 & __1__\n"); }
|
|
else if(mod2 & 2) { halWrite("\n mod:2 & __2__\n"); }
|
|
else if(mod2 & 3) { halWrite("\n mod:2 & __3__\n"); }
|
|
else if(mod2 & 4) { halWrite("\n mod:2 & __4__\n"); }
|
|
else if(mod2 & 5) { halWrite("\n mod:2 & __5__\n"); }
|
|
else if(mod2 & 6) { halWrite("\n mod:2 & __6__\n"); }
|
|
else if(mod2 & 7) { halWrite("\n mod:2 & __7__\n"); }
|
|
else if(mod2 & 8) { halWrite("\n mod:2 & __8__\n"); }
|
|
else if(mod2 & 9) { halWrite("\n mod:2 & __9__\n"); }
|
|
else if(mod2 & 10) { halWrite("\n mod:2 & __10_\n"); }
|
|
|
|
|
|
|
|
//Begin stop (old try...)
|
|
if(mod1 & 0x0020 || mod2 & 0x0001) {
|
|
train_set_speed(Train8, 0);
|
|
halWrite("\nStoppar tåg 78");
|
|
sleep(100);
|
|
switch_close(2);
|
|
set = 1;
|
|
}
|
|
if(set == 1)
|
|
if(mod1 != 0 || mod2 != 0)
|
|
if(!(mod1 & 0x0008) && !(mod2 & 0x0004) && !(mod2 & 0x0002)){
|
|
train_set_speed(Train8, 8);
|
|
halWrite("\nstartar tåg 78");
|
|
sleep(100);
|
|
switch_open(2);
|
|
set = 0;
|
|
}
|
|
if(mod1 & 0x0020 || mod2 & 0x0001) {
|
|
train_set_speed(Train8, 0);
|
|
|
|
|
|
*/
|
|
|
|
|