int main(void)
{
uint8_t count, second=0;
uint32_t val;
InitUART0 ();
InitRTC();
UART0_dbg_msg (
"********************************************************************************\n\r"
" Internal DAC test of LPC1788\n\r"
"\t - UART Comunication: 9600 bps \n\r"
" Write to debug console current voltage on AD[2]-AD[3]\n\r"
"********************************************************************************\n\r");
if (!InitADC (2))
{
UART0_dbg_msg ("InitADC exception, channel must be 0..7\n\r");
while (1);
}
InitDAC (0x03FF);
while (1)
{
//input DAC value
do
{
UART0_dbg_msg ("Input DAC value in range 0..1023, as a sample 0983\n\r");
while (!UART0_get_dec (&val,4))
UART0_dbg_msg ("DAC value is 10-bit number\n\r");
if (val>1024) {
UART0_dbg_msg ("DAC value isn't in range 0..1023\n\r");
UART0_clear_rx_buffer();
}
} while (val>1024);
count=0;
//Set DAC value
SetDAC(val);
//Convert DAC value through ADC 5 times
while(count<5)
{
if (second != LPC_RTC->SEC)
{
second=LPC_RTC->SEC;
ADC_dbg(GetADC());
count++;
}
}
}
}
int main(int argc, char** argv)
{
//Initializations
InitCLK();
InitGPIO();
InitDAC();
InitSPI();
InitTimer0();
InitWatchdog();
//Setup interrupts
PEIE = 1;
GIE = 1;
SPI_CS = CS_IDLE; //must be changed!
SSPCON1bits.SSPEN=0; // Disable SPI Port
//PORTCbits.RC5 = 0; //Set MOSI low
//PORTCbits.RC3 = 0; //Set SCK low
PORTCbits.PORTC = LATCbits.LATC & 0xD7; //set MOSI and SCK low
while(1)
{
SPI_CS = CS_IDLE; //hands off mode for testing the launcher
//Check if beacon has been launched
CheckDisconnect(); //DONE+TESTED
if(playbackFlag&&!MEM_ACCESS)
{
PlaybackMode();
playbackFlag = 0;
}
//Transmit message
if(transmitFlag) //DONE+TESTED
TransmitMode(); //DONE+TESTED
//Go back to sleep, wait for interrupts
Hibernate();
}
return (EXIT_SUCCESS);
}
int main(int argc, char* argv[])
{
UINT channel = 0;
unsigned char ModuleAddress;
int NumberOfUSB1s = 0;
BOOL blnResult;
// unsigned long TimeStamp;
//unsigned char ParallelInput;
DWORD start, finish, duration;
FILE *fp, *fn, *fw;
int i, j, k, ii, jj;
float tmp;
const int m = 200;
float e[3][m], u[3][m];
float Kp[3] = { 3.0, 5.0, 0.0 }, Kd[3] = { 0.1, 0.1, 0.0 }; // Position control
// float Kp[3] = {50.0, 3.0, 0.0}, Kd[3] ={1.00, 0.250, 0.0}; // Trajectory control
float theta[3][m], dtheta[3][m], de[3][m], thetad[3][m], dthetad[3][m], ddthetad[3][m];
long lVal[3];
double Kp_value[n], Kp_saturation = 50.0, KpVal[3][m];
float c[3] = { 10.0f, 10.0f, 0.0f }, s[3][m], Phi_p[3] = { 10.0f, 6.0f, 0.0f }, Phi_d[3] = { 0.5f, 1.0f, 0.0f };
float l = 1.0, h = 50.0, Enorm = 0.0f, Unorm = 0.0f;
float H2IW[DOF][number_I][number_H]; // Hidden layer weights
float O2HW[DOF][number_H + 1]; // Output layer weights
float H_Output[DOF][number_H + 1];
float TInput[DOF][number_I];
float NN_Output[DOF];
if ((fw = fopen("TrainedWts.txt", "rt")) == NULL)
{
printf("Unable to open TrainedWeights file!\n"); exit(0);
}
for (k = 0; k < DOF; k++) {
for (i = 0; i < number_I; i++)
{
for (j = 0; j < number_H; j++) fscanf(fw, "%f", &H2IW[k][i][j]);
}
for (i = 0; i < number_H + 1; i++)
{
fscanf(fw, "%f", &O2HW[k][i]);
}
}
// ************ Position Control Reference Trajectories *********
/*
for (i = 0; i < m; i++)
{
thetad[0][i] = 90.0f;
thetad[1][i] = 90.0f;
thetad[2][i] = 45.0f;
for (j = 0; j < DOF; j++) {
dthetad[j][i] = 0.0f;
ddthetad[j][i] = 0.0f;
}
}
*/
//
// ************ Trajectory Control Reference Trajectories: Cycloidal *********
for (i=0; i < m; i++)
{
if (i < m/2.55)
{
thetad[0][i] = 45.0f *(TS*3*i - sin(TS*3*i))/(2 * PI);
thetad[1][i] = 45.0f *(TS*3*i - sin(TS*3*i))/(2 * PI);
thetad[2][i] = 45.0f *(TS*3*i - sin(TS*3*i))/(2 * PI);
dthetad[0][i] = 40.0f *(3.0 - 3*cos(TS*3*i))/(2 * PI);
dthetad[1][i] = 45.0f *(3.0 - 3*cos(TS*3*i))/(2 * PI);
dthetad[2][i] = 45.0f *(3.0 - 3*cos(TS*3*i))/(2 * PI);
ddthetad[0][i] = - 360.0f * sin(TS*3*i)/(2 * PI);
ddthetad[1][i] = - 405.0f * sin(TS*3*i)/(2 * PI);
ddthetad[2][i] = - 405.0f * sin(TS*3*i)/(2 * PI);
} else
{
thetad[0][i] = 45.0f;
thetad[1][i] = 45.0f;
thetad[2][i] = 45.0f;
dthetad[0][i] = dthetad[1][i] = dthetad[2][i] = 0.0f;
ddthetad[0][i] = ddthetad[1][i] = ddthetad[2][i] = 0.0f;
}
}
// ************ Trajectory Control Reference Trajectories: Sine *********
// **********************************************************************
NumberOfUSB1s = USB1Init();
printf("Number of USB1s = %d\n", NumberOfUSB1s);
blnResult = USB1ReturnModuleAddress(0, &ModuleAddress);
if (blnResult == FALSE) printf("Cannot read Module Address!\n");
InitKusb();
for (i = 0; i < DOF; i++) {
e[i][0] = 0.0;
InitDAC();
/* printf("Initializing!\n");
USB1GetIncPosition(ModuleAddress, i, &lVal[i]);
if (i == 0 || i == 2) theta[i][0] = 360.0f*((float(lVal[i]%1600))/1600.0);
else theta[i][0] = 360.0f*((float(lVal[i]%1250))/1250.0);
while (theta[i][0] < 1.0f) DaOutput(2.0, channel+i);
*/
DaOutput(2.5, channel + i);
USB1GetIncPosition(ModuleAddress, i, &lVal[i]);
if (i == 0 || i == 2) theta[i][0] = 360.0f*((float(lVal[i] % 1600)) / 1600.0);
else theta[i][0] = 360.0f*((float(lVal[i] % 1250)) / 1250.0);
}
printf("Initial angles: %10.3f\t%10.3f\n", 360.0f*((float(lVal[0] % 1600)) / 1600.0), 360.0f*((float(lVal[1] % 1250)) / 1250.0));
for (i = 0; i < n; i++) Kp_value[i] = (i + 1) * (Kp_saturation) / n;
printf("Press ANY key to start!\n"); getch();
start = GetTickCount();
for (i = 1; i <= m; i++)
{
l = l + 1.0;
for (j = 0; j < DOF; j++)
{
USB1GetIncPosition(ModuleAddress, j, &lVal[j]);
if (j == 0 || j == 2) theta[j][i] = 360.0f*((float(lVal[j] % 1600)) / 1600.0);
else theta[j][i] = 360.0f*((float(lVal[j] % 1250)) / 1250.0);
e[j][i] = thetad[j][i] - theta[j][i];
dtheta[j][i] = Derivative(theta[j][i], theta[j][i - 1]);
de[j][i] = dthetad[j][i] - dtheta[j][i];
// ******************** Fuzzy Control ******************************
Kp[j] = KpFuzzyRule(e[j][i] * 6, Kp_value) / 10.0;
KpVal[j][i] = Kp[j];
// *****************************************************************
//
/* ******************** Sliding Mode Control ***********************
s[j][i] = c[j]*e[j][i] + de[j][i];
if ((s[j][i]*e[j][i]) >= 0.0) Kp[j] = Phi_p[j];
else Kp[j] = -Phi_p[j];
if ((s[j][i]*de[j][i]) >= 0.0) Kd[j] = Phi_d[j];
else Kd[j] = -Phi_d[j];
*/
u[j][i] = -(Kp[j] * e[j][i] + Kd[j] * de[j][i]) / 100;
// ******** Calculate Feedforward Compensation from Neural Network ******
// Calculate hidden layer neuron outputs
TInput[j][0] = theta[j][i]; TInput[j][1] = dtheta[j][i]; TInput[j][2] = ddthetad[j][i];
TInput[j][3] = 1.0;
for (ii = 0; ii < number_H; ii++)
{
tmp = 0.0;
for (k = 0; k <number_I; k++)
{
tmp = tmp + H2IW[j][k][ii] * TInput[ii][k];
}
H_Output[j][ii] = sigmoid(tmp);
}
H_Output[i][number_H] = 1.0f;
// Calculate NN output and error
for (j = 0; j < (number_H + 1); j++)
{
tmp = 0.0;
tmp = tmp + O2HW[i][j] * H_Output[i][j];
}
NN_Output[i] = tmp;
if (u[j][i] > 2.5) u[j][i] = 2.5; else if (u[j][i] < -2.5) u[j][i] = -2.5;
InitDAC();
DaOutput(u[j][i] + 2.5, channel + j);
}
Enorm = Enorm + e[0][i] * e[0][i] + e[1][i] * e[1][i]; Unorm = Unorm + u[0][i] * u[0][i] + u[1][i] * u[1][i];
}
finish = GetTickCount();
duration = finish - start;
Enorm = sqrt(Enorm); Unorm = sqrt(Unorm);
printf("\nTS = %d msec\t Enorm = %10.3f\t Unorm = %10.3f\n", duration / m, Enorm, Unorm);
printf("Initial angles: %10.3f\t%10.3f\n", 360.0f*((float(lVal[0] % 1600)) / 1600.0), 360.0f*((float(lVal[1] % 1250)) / 1250.0));
for (j = 0; j < 3; j++) {
InitDAC();
DaOutput(2.5, channel + j);
}
if ((fp = fopen("P.txt", "wt")) == NULL)
{
printf("Cannot open the output file!\n");
}
if ((fn = fopen("NnetTrainingData.txt", "wt")) == NULL)
{
printf("Cannot open the output file!\n");
}
// fprintf(fp, "\nTS = %d msec\t Enorm = %10.3f\t Unorm = %10.3f\n", duration/m, Enorm, Unorm);
for (i = 1; i < m; i++)
{
fprintf(fp, "%10.3f\t%10.3f\t%10.3f\t%10.3f\t%10.3f\t%10.3f\t%10.3f\t%10.3f\t%10.3f\t%10.3f\t%10.3f\n", i*TS, thetad[0][i], theta[0][i], e[0][i], u[0][i], KpVal[1][i], thetad[1][i], theta[1][i], e[1][i], u[1][i], KpVal[1][i]);
fprintf(fn, "%10.6f\t%10.6f\t%10.6f\t%10.6f\t%10.6f\t%10.6f\t%10.6f\t%10.6f\t%10.6f\t%10.6f\n", theta[0][i] / 57.3248, dtheta[0][i] / (40.0*57.3248), 0.0, 0.5*u[0][i], 1.0, theta[1][i] / 57.3248, dtheta[1][i] / (40.0*57.3248), 0.0, 0.5*u[1][i], 1.0);
}
rewind(fp);
fclose(fp);
InitDAC();
DaOutput(2.5, channel + 1);
olDaTerminate(board.hdrvr);
std::cin.get();
return((UINT)NULL);
}
int main(){ // the argument list is not used
// but there is provision for it.
printf("Welcome to the FIREBALL Guider. Please wait while we configure\n");
fflush(NULL);
StartLog();
GtoM_StartCount();
GtoT_StartCount();
ResetReceivedMessageCounters();
trackpointEL = CHIPHEIGHT/2;
trackpointCE = CHIPWIDTH/2;
QCamera camera; // open the camera.
// UniversalTime();
// printf("The Local Sidereal Time is: %lf\n",LST(31.761, -95.63));
/*
double ra1, ra2;
double dec1, dec2;
double alt1, alt2;
double az1, az2;
double del, dce;
double lat, lon;
*/
GtoT_CameraError(0);
SetLatLon(31.761,-95.64);
/* lat = 31.761;
lon = -95.64;
ra1 = 0.50;
ra2 = 0.51;
dec1 = 0.41;
dec2 = 0.41;
RADEC_to_ALTAZ(ra1,dec1, lat,lon, &alt1,&az1);
printf("the first alt-az I have found: %lf %lf\n",alt1,az1);
RADEC_to_ALTAZ(ra2,dec2, lat,lon, &alt2,&az2);
printf("the second alt-az I have found: %lf %lf\n",alt2,az2);
ALTAZ_to_ELCE(alt1,az1,alt2,az2,&del,&dce);
printf("The difference I have found is: %lf %lf\n",del,dce);
*/
InitDAC(); // prepare the dac.
InitializeLEDs(); // turn off the Lamp.
if(!ComSetup()) GtoT_TextError("COM working");
// RunSillyProg();
StartDisplay(); // begin the display.
turnOnGps();
// starts some of the statistic quantities. Defaulting to 100 frames -- 10s in full chip mode, 3s in ROI mode.
InitMode.setCallback(InitModeCallback);
AutocollimationMode.setCallback(AutocollimationModeCallback);
AlignmentMode.setCallback(AlignmentModeCallback);
AlignmentSubMode.setCallback(AlignmentModeCallback);
SlewingMode.setCallback(SlewPointModeCallback);
PointingMode.setCallback(SlewPointModeCallback);
PointingModeRoi.setCallback(SlewPointModeCallback);
camera.prepSettings(InitMode,0,0);
if(camera.LoadQCamState()){
switch(camera.nextMode.getModeID()){
case INITMODE:{
camera.nextMode.setCallback(InitModeCallback);
break;
};
case ALIGNMENTMODE:{
camera.nextMode.setCallback(AlignmentModeCallback);
alignmentMD.winx = camera.getWinX();
alignmentMD.winy = camera.getWinY();
alignmentMD.wind = camera.getWinDX();
WriteToLog("Stuff","%d %d %d",alignmentMD.winx,alignmentMD.winy,alignmentMD.wind);
break;
};
case SLEWINGMODE:{
camera.nextMode.setCallback(SlewPointModeCallback);
break;
};
case POINTINGMODE:{
camera.nextMode.setCallback(SlewPointModeCallback);
break;
};
case AUTOCOLLIMATIONMODE:{
camera.nextMode.setCallback(AutocollimationModeCallback);
break;
};
default:
break;
};
};
// camera.prepSettings(SlewingMode,0,0);
// camera.prepSettings(PointingMode,0,0);
// camera.nowMode.setEqual(AlignmentMode);
// HomeCamera();
//MoveStage(30000);
// periodicmessages.h/c contains the variables and calls needed to
// set up periodic messages that have to be sent to the MPF and the
// ground in various modes.
#include "periodicmessages.h"
// processmessages.h/c contains the variables and calls needed to
// process the messages from the MPF and the ground.
#include "processmessages.h"
//for every change of mode there will be a goto mainloop command
//to exit a program there will be a goto loopexit command.
FillRSquared();
// if(camera.getActiveSensor() == GUIDERSENSOR){
// GtoM_SwitchingGuiderRequest();
// };
// HelloDither.DitherPattern1(10);
MainLoop:
camera.stopStreaming(); // stop camera streaming
camera.changeSettings(); // adjust camera parameters, including
// the trigger time.
modeID = camera.getModeID();
camera.startStreaming(); // restart camera streaming.
// WriteToLog("Got Here! B ");
loopcounter = 0;
if(camera.getModeID() == POINTINGMODE || (camera.getModeID()==ALIGNMENTMODE && camera.getSubModeID()==ALIGNTRACKROI)){
ClearScreen();
FontOverwrite();
} else {
FontMask();
};
// WriteToLog("Got Here A !");
if(camera.getModeID()==ALIGNMENTMODE && (camera.getSubModeID() == ALIGNTRACKROI || camera.getSubModeID() == ALIGNTRACKFULLCHIP || camera.getSubModeID() == ALIGNDRAGFULLCHIP)){
// GtoM_SwitchingGuiderRequest();
// camera.setActiveSensor(GUIDERSENSOR);
} else {
// GtoM_SwitchingDTURequest();
// camera.setActiveSensor(OTHERSENSOR);
}
// WriteToLog("Got Here!");
// WriteToLog("FLOOD","%d %d %d" ,camera.getRoiX(), camera.getRoiY(),camera.getRoiDX());
do{
#include "periodicmessages.cpp"
#include "processmessages.cpp"
camera.queueFrame();
} while(1);
camera.~QCamera();
CloseLog();
return 0;
};
