Skip to content
Snippets Groups Projects
Commit e593c39a authored by Tom Feng's avatar Tom Feng
Browse files

attempt to add short distance, failed

parent a71e1efe
Branches backup727
No related tags found
3 merge requests!16Withtime,!15Samplek4,!14Shortdistancefailed
Hide whitespace changes
Inline Side-by-side
lib/display.c
+ 10
− 0
View file @ e593c39a
......@@ -396,6 +396,15 @@ int SensorStamp(TCServer,Num,SpeedDirSensor){
return SUCCESS;
}
void ShortByTick(TCServer,Num,SpeedDirSensor){
TCMsg SendMsg,ReplyMsg;
SendMsg.Type = TCSDTIME;
SendMsg.Num = Num;
SendMsg.TrainSpeed = SpeedDirSensor;
Send(TCServer,(void *)&SendMsg,sizeof(TCMsg),(void *)&ReplyMsg,sizeof(TCMsg));
}
void Display() {
char Ch;
char InputBuffer[CMD_SIZE];
......@@ -466,6 +475,7 @@ void Display() {
// Printf(COM2, "%d \n\r", Num);
break;
case ShortDist:
ShortByTick(TCServer,Num,SpeedDirSensor);
// Printf(COM2, "\n\rShortDist Num Dist\n\r");
// Printf(COM2, "%d %d\n\r", Num, SpeedDirSensor);
break;
......
lib/notifier.c
+ 1
− 1
View file @ e593c39a
......@@ -167,7 +167,7 @@ void TrainOutNotifier(){
BufferPopHead(&FIFO);
//bwprintf(COM2,"%c[%d;%dHTRON: AF2 %d %d %d \n\r",ESC,1,120,ReplyByte,FIFO.Length,FIFO.Head);
*(int *)(UART1_BASE) = ReplyByte;
Delay(CLKId,10);
Delay(CLKId,5);
//*(int *)(UART1_BASE | UART_CTLR_OFFSET) |= (1 << 5 | 1 << 3);
//bwprintf(COM2,"TrainOut: IRQ2:%d\n\r",(*(int *)(VIC2_BASE | INT_ENABLE)));
}
......
lib/tools.c
+ 48
− 0
View file @ e593c39a
......@@ -80,3 +80,51 @@ void setVel(Train* A){
void setStopDist(Train* A){
A->StopDist = (A->Vel * A->StopTick)/2;
}
int SDGetDistance(Train* A,int Tick){
int logresult = 0;
int frac = Tick/(A->SDMinTick);
int i = 1<<31;
int counter = 31;
for(;i|0;i>>=1){
if(i&frac) break;
counter --;
}
//Printf(COM2,"frac:%d Tick:%d Log: %d\n\r",frac,Tick,counter);
int Seven = 1;
for(i = counter; i>0; i--){
Seven *=7;
}
return 10*Seven;
}
int ABS(int Num){
if(Num<0) return -Num;
return Num;
}
int SDGetTicks(Train* A,int Distance){
int Upper = A->SDMaxTick;
int Lower = A->SDMinTick;
int last_dist = SDGetDistance(A,Lower);
int error = ABS(last_dist - Distance);
//int last_error = error;
//int last_tick = Lower;
int ticks = Lower;
while(1){
Printf(COM2,"%d %d %d %d %d\n\r",Upper,Lower,last_dist,error,ticks);
if(Lower>=Upper || Upper-Lower==1) return ticks;
if(last_dist < Distance){
Lower = ticks;
ticks = (ticks+Upper)/2;
}else if(last_dist > Distance){
Upper = ticks;
ticks = (Lower+ticks) /2;
}else if(last_dist == Distance) return ticks;
last_dist = SDGetDistance(A,ticks);
error = ABS(last_dist - Distance);
//last_tick = ticks;
//last_error = error;
}
}
lib/train-controller.c
+ 78
− 6
View file @ e593c39a
......@@ -23,13 +23,13 @@ void Watcher(){
int TCID = WhoIs("TCServer");
int TrainID, Sensor;
char* Sensors;
const int* Universal = (int *)GetShared();
TCS.Type = WATCHER;
Send(TCID,(void*)&TCS, sizeof(TCMsg),(void *)&TCR,sizeof(TCR));
TrainID = TCR.Num;
Sensor = TCR.TrainSpeed;
//Printf(COM2,"Watcher %d %d\n\r",Sensor>>3,(Sensor-((Sensor>>3)<<3)));
//Printf(COM2,"Watcher %d %d \n\r",TrainID,Sensor);
FOREVER{
......@@ -84,6 +84,64 @@ void Stamper(){
}
void ShortDistance(){
TCMsg TCS,TCR;
int TID = MyTid();
int Generation = GetGenerationINT(TID);
int ClkID = WhoIs("ClockServer");
int TCID = WhoIs("TCServer");
const int* Universal = (int *)GetShared();
TCS.Type = WATCHER;
Send(TCID,(void*)&TCS, sizeof(TCMsg),(void *)&TCR,sizeof(TCR));
int TrainID = TCR.Num;
int Tick = TCR.TrainSpeed;
Train* A = (Train *)TCR.SwitchDir;
//Printf(COM2,"%s%c[%d;%dHEstimateDistance:%d Train:%d%s",SAVE_CURSOR,ESC,12,80,SDGetDistance(A,Tick),A,RESTORE_CURSOR);
Printf(COM2,"%s%c[%d;%dHTimeStamp1:%d%s",SAVE_CURSOR,ESC,7,80,*(Universal),RESTORE_CURSOR);
TrainSetSpeed(TCID,TrainID,14);
Printf(COM2,"%s%c[%d;%dHTimeStamp2:%d%s",SAVE_CURSOR,ESC,8,80,*(Universal),RESTORE_CURSOR);
Delay(ClkID,Tick);
Printf(COM2,"%s%c[%d;%dHTimeStamp3:%d%s",SAVE_CURSOR,ESC,9,80,*(Universal),RESTORE_CURSOR);
TrainSetSpeed(TCID,TrainID,0);
Printf(COM2,"%s%c[%d;%dHTimeStamp4:%d%s",SAVE_CURSOR,ESC,10,80,*(Universal),RESTORE_CURSOR);
SelfDestroy();
}
void ShortDistanceTick(){
TCMsg TCS,TCR;
int TID = MyTid();
int Generation = GetGenerationINT(TID);
int ClkID = WhoIs("ClockServer");
int TCID = WhoIs("TCServer");
const int* Universal = (int *)GetShared();
TCS.Type = WATCHER;
Send(TCID,(void*)&TCS, sizeof(TCMsg),(void *)&TCR,sizeof(TCR));
int TrainID = TCR.Num;
int Distance = TCR.TrainSpeed;
Train* A = (Train *)TCR.SwitchDir;
int Tick = SDGetTicks(A,Distance);
Printf(COM2,"%s%c[%d;%dHDistance:%d Estimate Tick:%d%s",SAVE_CURSOR,ESC,12,80,Distance,Tick,RESTORE_CURSOR);
Printf(COM2,"%s%c[%d;%dHTimeStamp1:%d%s",SAVE_CURSOR,ESC,7,80,*(Universal),RESTORE_CURSOR);
TrainSetSpeed(TCID,TrainID,14);
Printf(COM2,"%s%c[%d;%dHTimeStamp2:%d%s",SAVE_CURSOR,ESC,8,80,*(Universal),RESTORE_CURSOR);
Delay(ClkID,Tick);
Printf(COM2,"%s%c[%d;%dHTimeStamp3:%d%s",SAVE_CURSOR,ESC,9,80,*(Universal),RESTORE_CURSOR);
TrainSetSpeed(TCID,TrainID,0);
Printf(COM2,"%s%c[%d;%dHTimeStamp4:%d%s",SAVE_CURSOR,ESC,10,80,*(Universal),RESTORE_CURSOR);
SelfDestroy();
}
//Printf(COM2,"Watcher %d %d \n\r",Train,Sensor);
void TCServer() {
int SenderID, Res, bReceived = 0;
int QStorage[MAX_NUM_TD];
......@@ -122,9 +180,11 @@ void TCServer() {
StopTrain = SensorID = -1;
//Printf(COM2,"TC:before loop\n\r");
Train SBB = {79,110.0,200.0,126.0,45.0,0,0,0};// calibrated
Train SBBBlack = {78,160.0,280.0,93.0,30.0,0,0};// calibrated
Train OCGate = {24,130.0,210.0,126.0,40.0,0,0,0};// calibrated
Train SBB = {79,110.0,200.0,126.0,45.0,0,0,0,0};// calibrated
Train SBBBlack = {78,160.0,280.0,93.0,30.0,0,0,0,0,0,80,300,0};// calibrated
Train OCGate = {24,130.0,210.0,126.0,40.0,0,0,0,0,0,50,300,0};// calibrated
OCGate.SDMinTick = 50;
OCGate.SDMaxTick = 300;
//Train EUEX = {58,3.4,0.45,0};// calibrated
FOREVER {
//SenderID = 0;
......@@ -191,18 +251,30 @@ void TCServer() {
Args.arg1 = (void *)(&Stamper);
WatcherID = Create(&Args);
Printf(COM2,"%s%c[%d;%dHStamperID:%d Index:%d%s\n\r",SAVE_CURSOR,ESC,3,80,WatcherID,GetMemoryIndexINT(WatcherID),RESTORE_CURSOR);
break;
case WATCHER:
if(StopTrain >=0 && SensorID >= 0){
ReplyMsg.Type = RESPONSE;
ReplyMsg.Num = StopTrain;
ReplyMsg.TrainSpeed = SensorID;
ReplyMsg.SwitchDir = (StopTrain == 24)? &OCGate:&SBBBlack;
Printf(COM2,"TC:%d\n\r",OCGate.SDMinTick);
Reply(SenderID, (void*)&ReplyMsg, sizeof(ReplyMsg));
StopTrain = -1;
SensorID = -1;
}
break;
case TCSDTIME:
ReplyMsg.Type = RESPONSE;
Reply(SenderID, (void*)&ReplyMsg, sizeof(ReplyMsg));
StopTrain = RcvMsg.Num;
SensorID = RcvMsg.TrainSpeed;
Args.arg0 = (void *) 10;
Args.arg1 = (void *)(&ShortDistance);
WatcherID = Create(&Args);
Printf(COM2,"%s%c[%d;%dHShortDistID:%d Index:%d%s\n\r",SAVE_CURSOR,ESC,3,80,WatcherID,GetMemoryIndexINT(WatcherID),RESTORE_CURSOR);
break;
case TEMP: //TODO
break;
default:
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment