void FORTE_BL_IX::executeEvent(int pa_nEIID){
switch(pa_nEIID){
case scm_nEventINITID:
delete m_poGPIO;
m_poGPIO = NULL;
if(QI() == true){
char * pacBuffer = new char[PARAMS().length() + 3];
PARAMS().toString(pacBuffer, sizeof(char)*(PARAMS().length() + 3));
if (verifyInput(pacBuffer, PARAMS().length() + 2)){
STATUS() = "OK";
QO() = true;
}else{
STATUS() = "Input PARAM error";
QO() = false;
}
delete[] pacBuffer;
}else{
STATUS() = "INIT- OK";
QO() = false;
}
sendOutputEvent(scm_nEventINITOID);
break;
case scm_nEventREQID:
if(m_poGPIO != NULL){
IN() = m_poGPIO->isHigh();
STATUS() = "OK";
}else{
IN() = false;
STATUS() = "Not initialized";
}
sendOutputEvent(scm_nEventCNFID);
break;
}
}
bool Device::checkCommandValue(){
if (EEPROMReadInt(PARAMS(deviceIdx, 2)) != FFFF) { // Threshold set
if (abs(previousCommandValue - commandValue) >= (EEPROMReadInt(PARAMS(deviceIdx, 2)))) {
previousCommandValue = commandValue;
return true;
}
}
return false;
}
int LIF_update(PVLayer *l)
{
int i;
float r = 0.0;
float phiAve = 0.0, phiMax = FLT_MIN, phiMin = FLT_MAX;
float VAve = 0.0, VMax = FLT_MIN, VMin = FLT_MAX;
for (i = 0; i < l->numNeurons; i++) {
if (pv_random_prob() < PARAMS(LIF_NOISE_FREQ)) {
r = PARAMS(LIF_NOISE_AMP) * 2 * (pv_random_prob() - 0.5);
}
else r = 0.0;
l->V[i] += PARAMS(LIF_DT_d_TAU) * (r + l->phi[PHI0][i] - l->V[i]);
l->V[i] = (l->V[i] < PARAMS(LIF_V_Min)) ? PARAMS(LIF_V_Min) : l->V[i]; // hard lower limit
// Gather some statistics
// TODO - take into account extended border
phiAve += l->phi[0][i];
if (l->phi[PHI0][i] < phiMin) phiMin = l->phi[PHI0][i];
if (l->phi[PHI0][i] > phiMax) phiMax = l->phi[PHI0][i];
VAve += l->V[i];
if (l->V[i] < VMin) VMin = l->V[i];
if (l->V[i] > VMax) VMax = l->V[i];
//pv_log("V1 PHI %d=%f\n", i, phi[i]);
// TODO - take into account extended border
l->phi[PHI0][i] = 0.0;
}
#ifdef DEBUG_OUTPUT
{
char msg[128];
sprintf(msg, "IF1: phi: Max: %1.4f, Avg=%1.4f Min=%1.4f\n", phiMax, phiAve
/ l->numNeurons, phiMin);
pv_log(stderr, msg);
sprintf(msg, "IF1: V : Max: %1.4f, Avg=%1.4f Min=%1.4f\n", VMax,
VAve / l->numNeurons, VMin);
pv_log(stderr, msg);
}
#endif
update_f(l);
return 0;
}
// Default handlers for a layer of leaky integrate-and-fire neurons.
static inline int update_f(PVLayer *l)
{
int i;
const float Vth = PARAMS(LIF_V_TH_0);
for (i = 0; i < l->numNeurons; i++) {
l->activity->data[i] = ((l->V[i] - Vth) > 0.0) ? 1.0 : 0.0;
l->V[i] -= l->activity->data[i] * l->V[i]; // reset cells that fired
}
return 0;
}
/*******************************************************************************
*** FUNCTIONCALL ()
*******************************************************************************
*** DESCRIPTION : Processes FUNCTIONCALL grammar rule.
***
*** FUNCTIONCALL -> idt ( PARAMS )
***
******************************************************************************/
void RecursiveParser::FUNCTIONCALL(TableEntry left1)
{
if (global->Token == Global::idt)
{
EntryPtr peek = symtab->lookup(global->Lexeme);
if (peek->TypeOfEntry == functionEntry)
{
string code = "call " + global->Lexeme + '\n';
EntryPtr left = symtab->lookup(left1.Lexeme);
if (left->depth <= 1)
code += left->Lexeme;
else
{
code += "[bp";
if (left->isParam)
code += "+";
else
code += "-";
code += NumberToString(left->var.Offset);
code += "]";
}
code += " = AX";
match(Global::idt);
match(Global::lparent);
PARAMS();
match(Global::rparent);
emit(code);
}
else
{
cout << "ERROR: Undefined function " << global->Lexeme << " at line " << lex->line_num << endl;
raise(SIGINT);
}
}
else
{
cout << "ERROR: Unexpected symbol " << global->Lexeme << " at line " << lex->line_num << endl;
raise(SIGINT);
}
}
bool CRaspiProcessInterface::initialise(bool m_bInputOrOutput){
wiringPiSetup();
CIEC_INT param;
param.fromString(PARAMS().getValue());
mPinNumber = param;
if(true == checkPin()){
if(m_bInputOrOutput){ //if pin used as input
pullUpDnControl(mPinNumber, PUD_DOWN); //pull-down the input-pin. use PUD_UP for pull-up.
CRaspiProcessInterface::smPinUsage[mPinNumber] = CRaspiProcessInterface::enInput;
pinMode(mPinNumber, INPUT);
}else{
CRaspiProcessInterface::smPinUsage[mPinNumber] = CRaspiProcessInterface::enOutput;
pinMode(mPinNumber, OUTPUT);
}
STATUS() = scmInitDeinitOK;
return true;
}else{
STATUS() = scmPinInUse;
return false;
}
}
//! Load settings from config and use them to set up mosaicer components, and set them going.
int main(int argc, char* argv[])
{
//grc::FeatureDetector * featureDetector = 0; //Delete these after (potentially) handling exceptions
grc::Renderer * mosaicRenderer = 0;
grc::EvaluationFunction * evaluationFunction = 0;
try
{
CMosaicingParams PARAMS(0, 0);
config config(argc > 1 ? argv[1] : "default.cfg");
PARAMS.init(&config);
PARAMS.BOW.DescriptorBinning.RADIUS = PARAMS.PatchDescriptor.radius();
boost::scoped_ptr<CImageSource> pImageLoader( CImageSource::makeImageSource( PARAMS.Im));
boost::scoped_ptr<CFeatureExtractor> pFeatureExtractor ( CFeatureExtractor::makeFeatureExtractor(PARAMS.Im, PARAMS.Corner, PARAMS.PatchDescriptor, PARAMS.DescriptorSetClustering));
grc::Transform::eTransType_s = (grc::Enums::eTransformType)(int)PARAMS.TransformType;
grc::Transform::warpMethod_s = (grc::Enums::eWarpMethod)(int)PARAMS.WarpMethod == grc::Enums::eWarpBilinear ? CV_INTER_LINEAR : CV_INTER_NN;
grc::cDescriptor::eDescriptorType_s = (grc::Enums::eDescriptorInvarianceType)(int)PARAMS.PatchDescriptorType;
grc::cDescriptor::eDescriptorSize_s = (grc::Enums::eDescriptorSize)(int)PARAMS.PatchDescriptorSize;
grc::ImageSource imSource((grc::Enums::eVideoSource)(int)PARAMS.VideoSource, PARAMS.Im.ImageDir.IMAGE_DIR.asSz(), PARAMS.Im.VideoFile.FILENAME.asSz(), (grc::Enums::eDisplayMode)(int)PARAMS.MosaicDestination, PARAMS.MosaicSaveDir.asSz()); //Pure-Translation-Extended "H:/Projects/External/DTA003 Image mosaicing/Test Data/Rangiora"
IplImage * pIm = cvLoadImage("/home/data/data/data/mosaicing/FixedCamera800/IMG_0004.JPG");
grc::FeatureMatcher2 * featureMatcher = 0;
//featureMatcher = new grc::BFFeatureMatcher(imSource, *pFeatureExtractor, PARAMS.BOWMatching);
CBoW bow(PARAMS.BOW);
featureMatcher = new grc::BoWFeatureMatcher(imSource, *pFeatureExtractor, bow, PARAMS.BOWMatching);
grc::BaySACTransformEstimator transformEstimator(*featureMatcher, PARAMS.RANSACHomography, PARAMS.Im.getCamCalibrationMat(),
((bool)PARAMS.MarkCorrespondences) ? pImageLoader.get() : 0);
switch(PARAMS.EvaluationFunction)
{
case grc::Enums::eSSDEvaluation:
evaluationFunction = new grc::SumSquaredErrorEvaluationFunction;
break;
}
if(evaluationFunction == 0 && PARAMS.EvaluationFunction != grc::Enums::eNoEvaluation)
throw new grc::GRCException("main: No evaluation function initialised");
CvSize mosaicSize = cvSize(PARAMS.MosaicX,PARAMS.MosaicY);
grc::TransformEngine engine(transformEstimator, PARAMS.MaxFrames, PARAMS.IncrementalRendering ? 1 : 0, PARAMS.LM * PARAMS.LMIterations,
mosaicSize, PARAMS.SkipFrames ? grc::Enums::eChooseSequentialSkip : grc::Enums::eChooseSequential, PARAMS.FullFrameUpdateFreq, PARAMS.MaxSearchForTransform);
switch(PARAMS.RendererType)
{
case grc::Enums::eBasicRenderer:
mosaicRenderer = new grc::Renderer(imSource, engine, mosaicSize, evaluationFunction);
break;
case grc::Enums::eFeatheredRenderer:
mosaicRenderer = new grc::FeatheredRenderer(imSource, engine, mosaicSize, PARAMS.FeatherRadius, evaluationFunction);
break;
case grc::Enums::eMultiScaleRenderer:
mosaicRenderer = new grc::MultiScaleFeatheredRenderer(imSource, engine, mosaicSize, PARAMS.FeatherRadius, evaluationFunction);
break;
case grc::Enums::eDijkstraRenderer:
mosaicRenderer = new grc::DijkstraCutRenderer(imSource, engine, mosaicSize, PARAMS.DijkstraScale, evaluationFunction);
break;
}
if(mosaicRenderer == 0)
throw new grc::GRCException("main: No renderer initialised");
//This ensures images are captured by this thread:
CStopWatch s;
s.startTimer();
imSource.doCaptureImages(&engine, mosaicRenderer);
s.stopTimer();
std::cout << s.getElapsedTime() << " seconds total" << endl;
//PARAMS.printUseSummary();
//PARAMS.printCfgFile();
}
catch(grc::GRCException * pEx)
{
std::cout << "ERROR: Unhandled exception: " << *(pEx->GetErrorMessage()) << std::endl << "Exiting...";
cvDestroyAllWindows();
#ifndef __GNUC__
Sleep(5000);
#endif
delete pEx;
}
//delete featureDetector;
delete mosaicRenderer;
delete evaluationFunction;
}
/**
* Run an fwd simulation optimization
*/
int main()
{
try {
//////////////////////////////////////////////////////////////////////////////////////////////
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%//
//////////////////////////////////////////////////////////////////////////////////////////////
SimTools* simtools = new SimTools(); // Instance of simtools class to use my functions
string osim_filename = "";
string force_filename_flex = "";
string marker_filename_flex = "";
string ik_setup_filename_flex = "";
string force_filename_ext = "";
string marker_filename_ext = "";
string ik_setup_filename_ext = "";
// Set up file directories
// Define osim file location
string osim_fd = "T:/Lamb expts 2012/Lamb experiments 2012/23rd - 24th July Expts/Opensim/Version9/";
// Specify force and trc mocap file
string fd = "T:/Lamb expts 2012/Lamb experiments 2012/23rd - 24th July Expts/Opensim/Lamb2 data files/";
osim_filename = osim_filename.append(osim_fd).append("Version9_contrained.osim"); // for flex
//Open existing XML model
Model osimModel(osim_filename);
Model osimModelE(osim_filename);
osimModel.printBasicInfo(cout);
//FLEXION
string expt_file_flex = "l2flexv2";
string output_fd_flex = "T:/Lamb expts 2012/Lamb experiments 2012/23rd - 24th July Expts/Opensim/Version9/flex_output.sto";
double tiFlex,tfFlex;
tiFlex = 2; // Static equilibrium
tfFlex = 15;
// EXTENSION
string expt_file_ext = "l2extv2a";
string output_fd_ext = "T:/Lamb expts 2012/Lamb experiments 2012/23rd - 24th July Expts/Opensim/Version9/ext_output.sto";
double tiExt,tfExt;
tiExt = 5;
tfExt = 18;
//flex filenames
force_filename_flex = force_filename_flex.append(fd).append(expt_file_flex).append(".mot");
marker_filename_flex = marker_filename_flex.append(fd).append(expt_file_flex).append(".trc");
ik_setup_filename_flex = ik_setup_filename_flex.append(osim_fd).append(expt_file_flex).append("_initial_ik.xml");
//ext filenames
force_filename_ext = force_filename_ext.append(fd).append(expt_file_ext).append(".mot");
marker_filename_ext = marker_filename_ext.append(fd).append(expt_file_ext).append(".trc");
ik_setup_filename_ext = ik_setup_filename_ext.append(osim_fd).append(expt_file_ext).append("_initial_ik.xml");
// Create storage object for force file
OpenSim::Storage* force_storage_ext = new Storage(force_filename_ext);
OpenSim::Storage* force_storage_flex = new Storage(force_filename_flex);
// smooths out all data....
force_storage_flex->smoothSpline(3,1); // order = 3, cutoff freq = 1Hz
force_storage_flex->resampleLinear(0.1); // resample to 10Hz
force_storage_ext->smoothSpline(3,1); // order = 3, cutoff freq = 1Hz
force_storage_ext->resampleLinear(0.1); // resample to 10Hz
//force_storage->print("T:/Lamb expts 2012/Lamb experiments 2012/23rd - 24th July Expts/Opensim/Version4/force_flex_smooth.sto");
//////////////////////////////////////////////////////////////////////////////////////////////
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%//
//////////////////////////////////////////////////////////////////////////////////////////////
//OpenSim::CoordinateSet& CS = osimModel.updCoordinateSet();
// Run initial ik step to find Initial conditions for forward simulation
Array<double> ICs_Flex, ICs_Ext;
osimModel.updCoordinateSet().get("t2ToGND_FE").setDefaultValue(0); // to flip up model so initial ik guess is close
osimModel.updCoordinateSet().get("t2ToGND_LB").setDefaultValue(0);
ICs_Ext = simtools->ik_constrain_torso(osimModel,marker_filename_ext,ik_setup_filename_ext,tiExt,tfExt);
cout<<"\n\next ic: :"<<ICs_Ext<<endl;
osimModel.print("T:/Lamb expts 2012/Lamb experiments 2012/23rd - 24th July Expts/Opensim/Version9/test1.osim");
OpenSim::CoordinateSet& CS = osimModel.updCoordinateSet();//.get("t2ToGND_FE").setDefaultIsPrescribed(false);
for(int i = 0; i<6; i++)
{
// First 6 coordinates in cooridnate set correspond to rotx,roty,rotz,tx,ty,tz
CS[i].setDefaultIsPrescribed(false);
CS[i].setDefaultValue(0.0);
}
// For Flex
osimModel.updCoordinateSet().get("t2ToGND_FE").setDefaultValue(-Pi/2); // to flip up model so initial ik guess is close
osimModel.updCoordinateSet().get("t2ToGND_LB").setDefaultValue(-Pi/2);
osimModel.print("T:/Lamb expts 2012/Lamb experiments 2012/23rd - 24th July Expts/Opensim/Version9/testa.osim");
// Returns state values at initial time -> ICs has length of coordinate set (computed by single ik step)
ICs_Flex = simtools->ik_constrain_torso(osimModel,marker_filename_flex,ik_setup_filename_flex,tiFlex,tfFlex);
osimModel.print("T:/Lamb expts 2012/Lamb experiments 2012/23rd - 24th July Expts/Opensim/Version9/test.osim");
cout<<"\n\nflex ic: :"<<ICs_Flex<<endl;
// Set up point kinematic analyses -> to track marker positions for use in objective function
//AnalysisSet pk_set =
//simtools->AddPKAtest(osimModel);
//cout<<"name : "<<osimModel.getNumAnalyses();
//cout<<"\n\nname : "<<osimModel.updAnalysisSet().get("m1");
////// USE force file to prescribe forces to head_markers body
////simtools->ApplyForce(*force_storage_flex,osimModel);
// Create measurement data structure for each simulation //
// Open trc file and store in class MarkerData
MarkerData md_flex = OpenSim::MarkerData(marker_filename_flex);
MarkerData md_ext = OpenSim::MarkerData(marker_filename_ext);
// Create storage object with marker data (md) in it
Storage data_trc_flex, data_trc_ext;
md_flex.makeRdStorage(data_trc_flex);
md_ext.makeRdStorage(data_trc_ext);
// crop storage object to only include data in time frame of simulation
data_trc_flex.crop(tiFlex,tfFlex); // -> data_trc is used to calculate rms error between model marker and motion capture markers
data_trc_ext.crop(tiExt,tfExt); // -> data_trc is used to calculate rms error between model marker and motion capture markers
//osimModel.print("T:/Lamb expts 2012/Lamb experiments 2012/23rd - 24th July Expts/Opensim/Version9/test.osim");
////////////////////////////////////////////////////////////////////////////////////////////////
////%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%//
////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////
// Create list of point kinematics reporters which match marker positions from osim model
PointKinematics* m1h = new PointKinematics(&osimModel);
PointKinematics* m2h = new PointKinematics(&osimModel);
PointKinematics* m3h = new PointKinematics(&osimModel);
PointKinematics* m4h = new PointKinematics(&osimModel);
PointKinematics* m5h = new PointKinematics(&osimModel);
PointKinematics* m6h = new PointKinematics(&osimModel);
// points in head_marker ref frame specified by inspection
Vec3 p1(0.071700000,0.00000000,-0.054772000);
Vec3 p2(-0.071700000,0.00000000,-0.038524000);
Vec3 p3(0.071700000,0.00000000,0.005228);
Vec3 p4(-0.0300000,0.00000000,0.026928);
Vec3 p5(0.0300000,0.00000000,0.026928);
Vec3 p6(-0.071700000,0.00000000,-0.008524000);
m1h->setBodyPoint("head_markers",p1);
m2h->setBodyPoint("head_markers",p2);
m3h->setBodyPoint("head_markers",p3);
m4h->setBodyPoint("head_markers",p4);
m5h->setBodyPoint("head_markers",p5);
m6h->setBodyPoint("head_markers",p6);
m1h->setRelativeToBody(&osimModel.updBodySet().get("ground"));
m2h->setRelativeToBody(&osimModel.updBodySet().get("ground"));
m3h->setRelativeToBody(&osimModel.updBodySet().get("ground"));
m4h->setRelativeToBody(&osimModel.updBodySet().get("ground"));
m5h->setRelativeToBody(&osimModel.updBodySet().get("ground"));
m6h->setRelativeToBody(&osimModel.updBodySet().get("ground"));
m1h->setName("m1");
m2h->setName("m2");
m3h->setName("m3");
m4h->setName("m4");
m5h->setName("m5");
m6h->setName("m6");
osimModel.addAnalysis(m1h);
osimModel.addAnalysis(m2h);
osimModel.addAnalysis(m3h);
osimModel.addAnalysis(m4h);
osimModel.addAnalysis(m5h);
osimModel.addAnalysis(m6h);
//////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////
//// RUN SIMULATION for a given set of bushing force properties
//// Define bushing properties
//// Define translational stiffness (t_stiff - x,y,z), rotational stiffness (r_stiff - x,y,z) and corresponding damping
Vec3 t_stiff(0),r_stiff(0.01),t_damp(0),r_damp(10);
//// initialise vector of initial parameter guesses
//int numParams = 4;
//Vector guess(numParams);
//guess[0] = 6.82;//6.00553; // theta star (degrees)
//guess[1] = 1.22; // k1 (N*m/degree)
//guess[2] = 7.29; // k2 (N*m/degree)
//guess[3] = 0.22;//0.409055; // damping (Nm/(deg/sec))
//Vector PARAMS(numParams);
//for (int i = 0; i<numParams; i++){
// PARAMS[i] = guess[i];
//}
////string fd = "";
//simtools->RunSimulation_FlexExt(osimModel,PARAMS,tiFlex,tiExt,tfFlex,tfExt,ICs_Flex,ICs_Ext,false,fd,*force_storage_flex,*force_storage_ext,*m1h,*m2h,*m3h,*m4h,*m5h,*m6h,data_trc_flex,data_trc_ext);
//
// %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% //
//
//// Initialise and run optimization
int numParams = 5;
//// Parameters:
//// - Theta_STAR
//// - k1
//// - k2
//// - damping
//MyOptimizerSystem sys(numParams,osimModel,data_trc,ti,tf,ICs,*m1h,*m2h,*m3h,*m4h,*m5h,*m6h,output_fd);
MyOptimizerSystem sys(numParams,osimModel,tiFlex,tiExt,tfFlex,tfExt,ICs_Flex,ICs_Ext,*force_storage_flex,*force_storage_ext,*m1h,*m2h,*m3h,*m4h,*m5h,*m6h,data_trc_flex,data_trc_ext);
Real f = NaN;
Vector lower_bounds(numParams);
Vector upper_bounds(numParams);
// theta star flex
lower_bounds[0] = 2;//0.0;//*Pi/180;
upper_bounds[0] = 12;//1.0;//*Pi/180;
// theta star ext
lower_bounds[1] = 2;//0.0;//*Pi/180;
upper_bounds[1] = 12;//1.0;//*Pi/180;
// k1
lower_bounds[2] = 1e-6;//0.0;//*Pi/180;
upper_bounds[2] = 15;//1.0;//*Pi/180;
// k2
lower_bounds[3] = 0.1;
upper_bounds[3] = 100;
// damping
lower_bounds[4] = -2;
upper_bounds[4] = 2;
//head mass
//lower_bounds[3] = -2.0;
//upper_bounds[3] = 2.0;
//// theta star sk
//lower_bounds[4] = 6;//0.0;//*Pi/180;
//upper_bounds[4] = 30;//1.0;//*Pi/180;
//// k1 sk
//lower_bounds[5] = 1e-6;//0.0;//*Pi/180;
//upper_bounds[5] = 15;//1.0;//*Pi/180;
//// k2 sk
//lower_bounds[6] = 0.1;
//upper_bounds[6] = 100;
// set parameter limits
sys.setParameterLimits(lower_bounds,upper_bounds);
// initialise vector of initial parameter guesses
Vector guess(numParams);
guess[0] = 6.82; //flexion theta star (degrees)
guess[1] = 6.82; // extension theta star (degrees)
guess[2] = 1.22; // k1 (N*m/degree)
guess[3] = 7.29; // k2 (N*m/degree)
guess[4] = 0.3; // bushing offset
//guess[4] = 0.0; // head mass
// guess[4] = 45; // theta star sk (degrees)
//guess[5] = guess[1]; // k1 sk (N*m/degree)
//guess[6] = guess[2]; // k2 sk (N*m/degree)
// Try optimisation
clock_t t1,t2,t3,t4;
t1 = clock();
try{
// intialise optimizer
Optimizer opt(sys, SimTK::InteriorPoint);
opt.setDiagnosticsLevel(5);
// Optimisation settings
opt.setConvergenceTolerance(1e-3);
opt.setMaxIterations(1000);
opt.useNumericalGradient(true);
opt.setLimitedMemoryHistory(500);
// return optimum solution
f = opt.optimize(guess);
cout<<"\nf = "<<f;
cout<<"\nguess = "<<guess;
}
catch(const std::exception& e) {
std::cout << "OptimizationExample.cpp Caught exception :" << std::endl;
std::cout << e.what() << std::endl;
}
t2 = clock();
//// %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% //
//cout<<"\n\nOptimiszation time: "<<((float)t2-(float)t1)/ CLOCKS_PER_SEC << " seconds";
//
//// Run simulation and save point kinematic reporter in data_sim storage object. Print simulation results to file.
//Array<Array<double>> pk_data;
Vector PARAMS(numParams);
for (int i = 0; i<numParams; i++){
PARAMS[i] = guess[i];
}
//string fd = "";
simtools->RunSimulation_FlexExt(osimModel,PARAMS,tiFlex,tiExt,tfFlex,tfExt,ICs_Flex,ICs_Ext,false,fd,*force_storage_flex,*force_storage_ext,*m1h,*m2h,*m3h,*m4h,*m5h,*m6h,data_trc_flex,data_trc_ext);
////
//////cout<<"\n\npk_data: "<<pk_data;
////t3 = clock();
////cout<<"\n\nSimulation time: "<<((float)t3-(float)t2)/ CLOCKS_PER_SEC << " seconds";
////double rms = simtools->Calc_rms_VERSION2(data_trc,ti,tf,pk_data);
////t4 = clock();
////cout<<"\n\nRMS time: "<<((float)t4-(float)t3)/ CLOCKS_PER_SEC << " seconds";
////cout<<"\n\nRMS ERROR: "<<rms;
////
////osimModel.print("T:/Lamb expts 2012/Lamb experiments 2012/23rd - 24th July Expts/Opensim/Version5/test.osim");
////ofstream myfile;
////myfile.open("T:/Lamb expts 2012/Lamb experiments 2012/23rd - 24th July Expts/Opensim/Version6/Model/fileConstrainedwBushingAndLimit.txt");
////double rms = 0;
////int n = 25;
////double val = 0;
////Vector k(n,(double)0);
////Vector obj(n,(double)0);
////for (int i=0; i<n; i++){
//// t1 = clock();
//// val = i*0.5 + 4;
//// t_slackL_opt = val;
//// //Vec3 r_stiff_opt(val);
//// pk_data = simtools->RunSimulation_LIMITSTOP(osimModel,t_stiff,r_stiff_opt,t_damp,r_damp,ti,tf,t_slackL_opt,vert_mass_opt,head_mass_opt,ICs,false,osim_fd,*m1h,*m2h,*m3h,*m4h,*m5h,*m6h,fibreL_opt);
//// rms = simtools->Calc_rms_VERSION2(data_trc,ti,tf,pk_data);
//// k[i] = val;
//// obj[i] = rms;
//// cout<<i<<"\n";
//// myfile<<k[i]<<"\t"<<obj[i]<<"\n";
//// t2 = clock();
//// cout<<k[i]<<"\t"<<obj[i]<<"\n";
//// cout<<"\n\nLoop time: "<<((float)t2-(float)t1)/ CLOCKS_PER_SEC << " seconds";
////}
////myfile.close();
////cout<<"\n\nk = "<<k;
////cout<<"\n\nobj = "<<obj;
//
////ofstream myfile_2param;
////myfile_2param.open("T:/Lamb expts 2012/Lamb experiments 2012/23rd - 24th July Expts/Opensim/Version6/Model/fileConstrainedwBushingAndLimit_2param_v2.txt");
////double rms = 0;
////int n =10;
////int nn = 20;
////double val_k = 0, val_m = 0;
////Vector k(n*nn,(double)0);
////Vector m(n*nn,(double)0);
////Vector obj(n*nn,(double)0);
////for (int i=0; i<n; i++){
//// for (int j=0;j<nn; j++){
//// val_k = i*0.05 + 0.3;
//// val_m = j*0.5 + 5;
//// t_slackL_opt = val_m;
//// head_mass_opt = val_k;
//// //Vec3 r_stiff_opt(val_k);
//// t_slackL_opt = val_m;
//// pk_data = simtools->RunSimulation_LIMITSTOP(osimModel,t_stiff,r_stiff_opt,t_damp,r_damp,ti,tf,t_slackL_opt,vert_mass_opt,head_mass_opt,ICs,false,osim_fd,*m1h,*m2h,*m3h,*m4h,*m5h,*m6h,fibreL_opt);
//// rms = simtools->Calc_rms_VERSION2(data_trc,ti,tf,pk_data);
//// k[i*n + j] = val_k;
//// m[i*n + j] = val_m;
//// obj[i*n + j] = rms;
//// cout<<(i)<<"\t"<<j<<"\n";
//// cout<<k[i*n + j]<<"\n";
//// cout<<m[i*n + j]<<"\n";
//// cout<<rms<<"\n";
//// myfile_2param<<k[i*n + j]<<"\t"<<m[i*n + j]<<"\t"<<obj[i*n + j]<<"\n";
//// }
////}
////myfile_2param.close();
////cout<<"\n\nk = "<<k;
////cout<<"\n\nm = "<<m;
////cout<<"\n\nobj = "<<obj;
////////////////////////////////////////////////////////////////////////////////////////////////
////%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%//
////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
}
catch (OpenSim::Exception ex)
{
std::cout << ex.getMessage() << std::endl;
return 1;
}
catch (std::exception ex)
{
std::cout << ex.what() << std::endl;
return 1;
}
catch (...)
{
std::cout << "UNRECOGNIZED EXCEPTION" << std::endl;
return 1;
}
//std::cout << "main() routine time = " << 1.e3*(std::clock()-startTime)/CLOCKS_PER_SEC << "ms\n";
std::cout << "\n\nOpenSim example completed successfully.\n";
std::cin.get();
return 0;
}
void printPage(const byte clientsel, const byte deviceIdx) {
// Index
printP(clientsel, TXTIND);
printP(clientsel, TXTCOLON);
printV(clientsel, deviceIdx);
printP(clientsel, ANBSP);
// DeviceID
printP(clientsel, TXTDEVICEID);
printP(clientsel, TXTCOLON);
printV(clientsel, mdevices[deviceIdx].getDeviceID());
// Type
printP(clientsel, ANBSP);
if (mdevices[deviceIdx].getType() != 0) {
printP(clientsel, TXTTYPE);
printP(clientsel, TXTCOLON);
printV(clientsel, mdevices[deviceIdx].getType());
printP(clientsel, AOPEN);
printP(clientsel, BR);
printP(clientsel, SLASH);
printP(clientsel, ACLOSE);
}
// CommandID
printP(clientsel, TXTCOMMAND );
printP(clientsel, TXTCOLON);
printVstr(clientsel, mdevices[deviceIdx].getCommand());
// Status
printP(clientsel, ANBSP);
printP(clientsel, TXTSTATUS );
printP(clientsel, TXTCOLON);
printVstr(clientsel, mdevices[deviceIdx].getStatus());
// Value
if (mdevices[deviceIdx].commandValue != 0) {
printP(clientsel, ANBSP);
printP(clientsel, TXTVALUE );
printP(clientsel, TXTCOLON);
printVstr(clientsel, mdevices[deviceIdx].getValue());
}
// ExtData
if (printVstr(clientsel, mdevices[deviceIdx].getExtData(), true) > 0) {
printP(clientsel, AOPEN);
printP(clientsel, BR);
printP(clientsel, SLASH);
printP(clientsel, ACLOSE);
printP(clientsel, TXTEXTDATA );
printP(clientsel, TXTCOLON);
printVstr(clientsel, mdevices[deviceIdx].getExtData());
}
printP(clientsel, AOPEN);
printP(clientsel, BR);
printP(clientsel, SLASH);
printP(clientsel, ACLOSE);
if (EEPROMReadInt(PARAMS(deviceIdx, 1)) != FFFF) {
// Parameter 1
printP(clientsel, TXTPAR1);
printP(clientsel, TXTCOLON);
printV(clientsel, EEPROMReadInt(PARAMS(deviceIdx, 1)));
printP(clientsel, AOPEN);
printP(clientsel, BR);
printP(clientsel, SLASH);
printP(clientsel, ACLOSE);
}
if (EEPROMReadInt(PARAMS(deviceIdx, 2)) != FFFF) {
// Parameter 2
printP(clientsel, TXTPAR2);
printP(clientsel, TXTCOLON);
printV(clientsel, EEPROMReadInt(PARAMS(deviceIdx, 2)));
printP(clientsel, AOPEN);
printP(clientsel, BR);
printP(clientsel, SLASH);
printP(clientsel, ACLOSE);
}
if (EEPROMReadInt(PARAMS(deviceIdx, 3)) != FFFF) {
// Parameter 3
printP(clientsel, TXTPAR3);
printP(clientsel, TXTCOLON);
printV(clientsel, EEPROMReadInt(PARAMS(deviceIdx, 3)));
printP(clientsel, AOPEN);
printP(clientsel, BR);
printP(clientsel, SLASH);
printP(clientsel, ACLOSE);
}
if (EEPROMReadInt(PARAMS(deviceIdx, 4)) != FFFF) {
// Parameter 3
printP(clientsel, TXTPAR4);
printP(clientsel, TXTCOLON);
printV(clientsel, EEPROMReadInt(PARAMS(deviceIdx, 4)));
printP(clientsel, AOPEN);
printP(clientsel, BR);
printP(clientsel, SLASH);
printP(clientsel, ACLOSE);
}
if (EEPROMReadInt(PARAMS(deviceIdx, 5)) != FFFF) {
// Parameter 5
printP(clientsel, TXTPAR5);
printP(clientsel, TXTCOLON);
printV(clientsel, EEPROMReadInt(PARAMS(deviceIdx, 5)));
printP(clientsel, AOPEN);
printP(clientsel, BR);
printP(clientsel, SLASH);
printP(clientsel, ACLOSE);
}
// In deviceIdx
if (mdevices[deviceIdx].getInput() != 0) {
printP(clientsel, TXTDEVIND);
printP(clientsel, TXTCOLON);
printV(clientsel, mdevices[deviceIdx].getInput());
printP(clientsel, AOPEN);
printP(clientsel, BR);
printP(clientsel, SLASH);
printP(clientsel, ACLOSE);
}
// Pin
if (mdevices[deviceIdx].getPin() != 0) {
printP(clientsel, TXTPIN);
printP(clientsel, TXTCOLON);
printV(clientsel, mdevices[deviceIdx].getPin());
printP(clientsel, AOPEN);
printP(clientsel, BR);
printP(clientsel, SLASH);
printP(clientsel, ACLOSE);
}
if (DEBUG_WEB) dbg.println();
return;
}
void Device::readInput() {
/*!
* Reads current input value of device
*/
commandValue = 0;
int prevRunning;
switch (type) {
case TYPE_DIGITAL_IO:
case TYPE_DIGITAL_IO_NEG:
status = digitalRead(getPin());
break;
case TYPE_ANALOG_IN:
case TYPE_NTC:
commandValue = analogRead(getPin());
if (type == TYPE_NTC) commandValue = 1024 - commandValue;
if (EEPROMReadInt(PARAMS(deviceIdx, 1)) == FFFF) { // No setpoint set
status = STATUS_UNKNOWN;
sprintf(temp, "{\"V\":\"%i\"}", commandValue);
setExtData(temp);
} else {
if (commandValue > EEPROMReadInt(PARAMS(deviceIdx, 1))) { // Above set point
status = STATUS_ON;
}
if (commandValue <= (EEPROMReadInt(PARAMS(deviceIdx, 1)) - EEPROMReadInt(PARAMS(deviceIdx, 2)))) { // Below set point and threshold
status = STATUS_OFF;
}
sprintf(temp, "{\"V\":\"%i\",\"S\":\"%u\",\"T\":\"%u\"}", commandValue, EEPROMReadInt(PARAMS(deviceIdx, 1)), EEPROMReadInt(PARAMS(deviceIdx, 2)));
setExtData(temp);
checkStatusOrCommandValue();
}
break;
case TYPE_DHT22:
byte chk;
status = STATUS_ERROR;
chk = dht.read(getPin());
switch (chk) {
case DHTLIB_OK:
commandValue = ((int)dht.temperature);
int temp1;
int temp2;
temp1 = abs((dht.temperature - (int)dht.temperature) * 100);
temp2 = (dht.humidity - (int)dht.humidity) * 100;
if (EEPROMReadInt(PARAMS(deviceIdx, 1)) == FFFF) { // No setpoint set
status = STATUS_UNKNOWN;
sprintf(temp, "{\"T\":\"%0d.%d\",\"H\":\"%0d.%d\"}", (int)dht.temperature, temp1, (int)dht.humidity, temp2);
setExtData(temp);
} else {
if (commandValue > EEPROMReadInt(PARAMS(deviceIdx, 1))) { // below set point
status = STATUS_ON;
}
if (commandValue <= (EEPROMReadInt(PARAMS(deviceIdx, 1)) - EEPROMReadInt(PARAMS(deviceIdx, 2)))) { // above set point plus threshold
status = STATUS_OFF;
}
sprintf(temp, "{\"T\":\"%0d.%d\",\"H\":\"%0d.%d\",\"S\":\"%u\"}", (int)dht.temperature, temp1, (int)dht.humidity, temp2, EEPROMReadInt(PARAMS(deviceIdx, 1)));
setExtData(temp);
checkStatusOrCommandValue();
}
break;
default:
status= STATUS_ERROR;
setExtData("");
break;
}
break;
case TYPE_THERMO_HEAT:
case TYPE_THERMO_COOL:
commandValue = readTemp(getInput());
prevRunning = digitalRead(getPin());
if (status) { // Enabled or Not
if (commandValue != ERROR) { // Could not read temp, leave as is
if (getType() == TYPE_THERMO_COOL) {
if (commandValue >= EEPROMReadInt(PARAMS(deviceIdx, 1))) { // Switch on if above set point
digitalWrite(getPin(), HIGH);
}
if (commandValue <= (EEPROMReadInt(PARAMS(deviceIdx, 1)) - EEPROMReadInt(PARAMS(deviceIdx, 2)))) { // Switch off if below threshold
digitalWrite(getPin(), LOW);
}
}
if (getType() == TYPE_THERMO_HEAT) {
if (commandValue <= EEPROMReadInt(PARAMS(deviceIdx, 1))) { // Switch on if below set point
digitalWrite(getPin(), HIGH);
}
if (commandValue >= (EEPROMReadInt(PARAMS(deviceIdx, 1)) + EEPROMReadInt(PARAMS(deviceIdx, 2)))) { // Switch off if above threshold
digitalWrite(getPin(), LOW);
}
}
} // If Error read fall trough
} else {
digitalWrite(mdevices[deviceIdx].getPin(), LOW);
}
sprintf(temp, "{\"V\":\"%i\",\"R\":\"%i\",\"S\":\"%u\",\"T\":\"%u\"}", commandValue, digitalRead(getPin()), EEPROMReadInt(PARAMS(deviceIdx, 1)), EEPROMReadInt(PARAMS(deviceIdx, 2)));
setExtData(temp);
if ((prevRunning != digitalRead(getPin())) || checkCommandValue()) {
int previousCommandID = commandID;
setCommand(COMMAND_SET_RESULT);
postMessage(deviceIdx);
commandID = previousCommandID;
}
break;
case TYPE_AUTO_DOOR:
if (!digitalRead(TOP_SWITCH_PIN)) {
status = STATUS_ON;
} else if (!digitalRead(BOTTOM_SWITCH_PIN)) {
status = STATUS_OFF;
} else {
status = STATUS_UNKNOWN;
}
sprintf(temp, "{\"T\":\"%i\",\"B\":\"%i\",\"P\":\"%i\",\"D\":\"%i\"}", digitalRead(TOP_SWITCH_PIN), digitalRead(BOTTOM_SWITCH_PIN), digitalRead(POWER_RELAY_PIN), digitalRead(DIRECTION_RELAY_PIN));
setExtData(temp);
checkStatus();
break;
case TYPE_ARDUINO:
status = digitalRead(getPin());
sprintf(temp, "{\"R\":\"%i\",\"M\":\"%lu\",\"U\":\"%lu\"}", RELEASE, check_mem(), millis()/1000);
setExtData(temp);
checkStatus();
break;
default:
break;
}
}
void FORTE_BL_PWM::executeEvent(int pa_nEIID){
bool bRet = true;
switch(pa_nEIID){
case scm_nEventINITID:
if(m_poPWM != NULL){
//Fix output before stopping the PWM
//TODO:Handle the PWM output level when stopping it
m_poPWM->setRunState(BlackLib::stop);
delete m_poPWM;
m_poPWM = NULL;
}
if(QI() == true){
char * pacBuffer = new char[PARAMS().length() + 3];
PARAMS().toString(pacBuffer, sizeof(char)*(PARAMS().length() + 3));
if (strncmp(pacBuffer, "\'EHRPWM2B\'", 10) == 0) {
m_poPWM = new BlackLib::BlackPWM(BlackLib::EHRPWM2B);
} else if (strncmp(pacBuffer, "\'EHRPWM2A\'", 10) == 0) {
m_poPWM = new BlackLib::BlackPWM(BlackLib::EHRPWM2A);
} else if (strncmp(pacBuffer, "\'EHRPWM1A\'", 10) == 0) {
m_poPWM = new BlackLib::BlackPWM(BlackLib::EHRPWM1A);
} else if (strncmp(pacBuffer, "\'EHRPWM1B\'", 10) == 0) {
m_poPWM = new BlackLib::BlackPWM(BlackLib::EHRPWM1B);
} else if (strncmp(pacBuffer, "\'EHRPWM0B\'", 10) == 0) {
m_poPWM = new BlackLib::BlackPWM(BlackLib::EHRPWM0B);
} else if (strncmp(pacBuffer, "\'EHRPWM0A\'", 10) == 0) {
m_poPWM = new BlackLib::BlackPWM(BlackLib::EHRPWM0A);
} else if (strncmp(pacBuffer, "\'ECAP0\'", 7) == 0) {
m_poPWM = new BlackLib::BlackPWM(BlackLib::ECAP0);
} else {
delete[] pacBuffer;
DEVLOG_ERROR("Invalid input PARAM in BL_PWM FB\n");
STATUS() = "Input PARAM error";
QO() = false;
sendOutputEvent(scm_nEventINITOID);
break;
}
delete[] pacBuffer;
bRet = m_poPWM->setRunState(BlackLib::stop);
bRet = bRet && m_poPWM->setDutyPercent(0.0);
bRet = bRet && m_poPWM->setPeriodTime(Period().getInMicroSeconds(), BlackLib::microsecond);
bRet = bRet && m_poPWM->setDutyPercent(Duty());
if(Polarity() == true){
bRet = bRet && m_poPWM->setPolarity(BlackLib::straight);
}else{
bRet = bRet && m_poPWM->setPolarity(BlackLib::reverse);
}
bRet = bRet && m_poPWM->setRunState(BlackLib::run);
if(bRet == true){
//PWM configuration OK
STATUS() ="OK";
QO() = true;
}else{
//PWM configuration Error
DEVLOG_DEBUG("Invalid configuration parameters in BL_PWM FB\n");
STATUS() ="Configuration Error";
QO() = false;
}
}else{
STATUS() = "INIT- OK";
QO() = false;
}
sendOutputEvent(scm_nEventINITOID);
break;
case scm_nEventUpdateID:
if (m_poPWM == NULL){
//PWM must be initialized before updating configuration
DEVLOG_ERROR("Trying to update before initializing PWM\n");
STATUS() = "ERROR updating PWM";
QO()=false;
sendOutputEvent(scm_nEventCNFID);
break;
}
bRet = m_poPWM->setDutyPercent(0.0);
bRet = bRet && m_poPWM->setPeriodTime(Period().getInMicroSeconds(), BlackLib::microsecond);
bRet = bRet && m_poPWM->setDutyPercent(Duty());
if(Polarity() == false){
//BlackLib polarity seems to be inverse; For us straight is HIGH->LOW
bRet = bRet && m_poPWM->setPolarity(BlackLib::straight);
}else{
bRet = bRet && m_poPWM->setPolarity(BlackLib::reverse);
}
if(bRet == true){
//PWM configuration OK
STATUS() ="OK";
QO() = true;
}else{
//PWM configuration Error
DEVLOG_DEBUG("Invalid configuration parameters in BL_PWM FB\n");
STATUS() ="Configuration Error";
QO() = false;
}
sendOutputEvent(scm_nEventCNFID);
break;
case scm_nEventStartID:
if(m_poPWM != NULL){
if(m_poPWM->setRunState(BlackLib::run)){
QI() = true;
STATUS() = "Running";
}else{
//Could not start PWM
DEVLOG_ERROR("Unable to run PWM\n");
STATUS() = "ERROR running PWM";
QO()=false;
}
}else{
QI() = false;
STATUS() = "PWM not initialized";
}
sendOutputEvent(scm_nEventCNFID);
break;
case scm_nEventStopID:
if (m_poPWM != NULL){
//TODO:Handle the PWM output level when stopping it
if(m_poPWM->setRunState(BlackLib::stop)){
STATUS() = "PWM Stopped";
QO()=true;
}else{
//Could not stop PWM
DEVLOG_ERROR("Unable to stop PWM\n");
STATUS() = "ERROR stopping PWM";
QO()=false;
}
}else{
QI() = false;
STATUS() = "PWM not initialized";
}
sendOutputEvent(scm_nEventCNFID);
break;
}
}
//funkce na vkladani parametru
tChyba PARAMS() {
int analyza;
//PARAMS->( id : DTYPE PARAMS
if(token.stav == s_leva_zavorka) {
token = getNextToken();
if(token.stav == s_lex_error) {
return S_LEXIKALNI_CHYBA;
}
else if(token.stav == s_identifikator) {
if((params = malloc(strlen(token.data)+1)) == NULL) {
return S_INTERNI_CHYBA;
}
//do globalni promenne params nacteme id parametru funkce
strcpy(params, token.data);
token = getNextToken();
if(token.stav == s_lex_error) {
return S_LEXIKALNI_CHYBA;
}
if(token.stav == s_dvojtecka) {
token = getNextToken();
if(token.stav == s_lex_error) {
return S_LEXIKALNI_CHYBA;
}
analyza = DTYPE();
if(analyza != S_BEZ_CHYB) {
return analyza;
}
if(!strcmp(token.data, "integer")) {
dtype = TYPEINT;
}
else if(!strcmp(token.data, "real")) {
dtype = TYPEDOUBLE;
}
else if(!strcmp(token.data, "string")) {
dtype = TYPESTR;
}
else if(!strcmp(token.data, "boolean")) {
dtype = TYPEBOOL;
}
//vlozeni do pole parametru
htParamInsert(ptrhtGlobal,funkce, params, dtype);
//vlozeni parametru do lokalni hash
htInsert(func[currFuncSize].table, params, NULL, dtype, ID_PARAM);
free(params);
token = getNextToken();
if(token.stav == s_lex_error) {
return S_LEXIKALNI_CHYBA;
}
//rekurzivni volani
return PARAMS();
}
return S_SYNTAKTICKA_CHYBA;
}
}//PARAMS->; id : DTYPE PARAMS
else if(token.stav == s_strednik) {
token = getNextToken();
if(token.stav == s_lex_error) {
return S_LEXIKALNI_CHYBA;
}
if(token.stav == s_identifikator) {
if((params = malloc(strlen(token.data)+1)) == NULL) {
return S_INTERNI_CHYBA;
}
//vlozeni klice do globalni promenne
strcpy(params, token.data);
token = getNextToken();
if(token.stav == s_lex_error) {
return S_LEXIKALNI_CHYBA;
}
else if(token.stav == s_dvojtecka) {
token = getNextToken();
if(token.stav == s_lex_error) {
return S_LEXIKALNI_CHYBA;
}
analyza = DTYPE();
if(analyza != S_BEZ_CHYB) {
return analyza;
}
if(!strcmp(token.data, "integer")) {
dtype = TYPEINT;
}
else if(!strcmp(token.data, "string")) {
dtype = TYPESTR;
}
else if(!strcmp(token.data, "real")) {
dtype = TYPEDOUBLE;
}
else if(!strcmp(token.data, "boolean")){
dtype = TYPEBOOL;
}
//vlozeni do globalni, do pole parametru funkce
htParamInsert(ptrhtGlobal,funkce, params, dtype);
htInsert(func[currFuncSize].table, params, NULL, dtype, ID_PARAM);
free(params);
token = getNextToken();
if(token.stav == s_lex_error) {
return S_LEXIKALNI_CHYBA;
}
return PARAMS();
}
}
return S_SYNTAKTICKA_CHYBA;
}
else if(token.stav == s_prava_zavorka) {
token = getNextToken();
if(token.stav == s_lex_error) {
return S_LEXIKALNI_CHYBA;
}
return S_BEZ_CHYB;
}
return S_SYNTAKTICKA_CHYBA;
}
tChyba FUNKCE() {
int analyza;
//opet lokalni promenna, stejna funkce jako ve funkci nahore
TItem *nasel;
// FUNKCE-> begin ACT_LIST
if(!strcmp(token.data, "begin") && token.stav == s_klicove) {
token = getNextToken(); //pres strcmp porovnavam data tokenu (prakticky primo to, co se nacte)
if(token.stav == s_lex_error) {
return S_LEXIKALNI_CHYBA;
}
//po zavolani funkce se getToken nevola, protoze kazda funkce, nez vrati hodnotu, nacte dalsi token
analyza = ACT_LIST();
if(analyza != S_BEZ_CHYB) {
return analyza;
}
return S_BEZ_CHYB;
}
//FUNKCE->function id PARAMS : DTYPE ; DOPREDNE ; FUNKCE
else if(!strcmp(token.data, "function") && token.stav == s_klicove) {
//pamatujeme si, ze jsme v definici funkce
byla_funkce = true;
token = getNextToken();
if(token.stav == s_lex_error) {
return S_LEXIKALNI_CHYBA;
}
if(token.stav == s_identifikator) {
//pokud id ma nazev stejny jako tyto 2 funkce, jedna se o chybu
if(!strcmp(token.data, "length") || !strcmp(token.data, "copy")) {
return S_SEMANTICKA_CHYBA_NEDEF;
}
if((funkce = malloc(strlen(token.data)+1)) == NULL) {
return S_INTERNI_CHYBA;
}
//nulovani poctu parametru funkce
TData *dat = (TData*)malloc(sizeof(TData));
dat->param.numParam = 0;
strcpy(funkce, token.data);
//pokud id funkce j*z je v globalni hash tabulce a nejedna se o doprednou deklaraci -> chyba
nasel = htSearch(ptrhtGlobal,funkce);
if(nasel != NULL ) {
if(nasel->init == true) {
return S_SEMANTICKA_CHYBA_NEDEF;
}
nasel->init = true;
}
//vlozeni nazvu funkce do globalni hash
htInsert(ptrhtGlobal,funkce,dat, TYPEUNDEF , ID_FUNCTION);
token = getNextToken();
if(token.stav == s_lex_error) {
return S_LEXIKALNI_CHYBA;
}
//funkce na kontrolu a ukladani parametru funkce
analyza = PARAMS();
if(analyza != S_BEZ_CHYB) {
return analyza;
}
else if(token.stav == s_dvojtecka) {
token = getNextToken();
if(token.stav == s_lex_error) {
return S_LEXIKALNI_CHYBA;
}
//funkce na kontrolu datovych typu
analyza = DTYPE();
if(analyza != S_BEZ_CHYB) {
return analyza;
}
if(!strcmp(token.data, "integer")) {
dtype = TYPEINT;
}
else if(!strcmp(token.data, "real")) { //do docasne promene nacteme datovy typ funkce
dtype = TYPEDOUBLE;
}
else if(!strcmp(token.data, "string")) {
dtype = TYPESTR;
}
else if(!strcmp(token.data, "boolean")) {
dtype = TYPEBOOL;
}
token = getNextToken();
if(token.stav == s_lex_error) {
return S_LEXIKALNI_CHYBA;
}
else if(token.stav == s_strednik) {
token = getNextToken();
if(token.stav == s_lex_error) {
return S_LEXIKALNI_CHYBA;
}
//zjisteni, zda se jedna o doprednou deklaraci funkces
analyza = DOPREDNE();
if(analyza != S_BEZ_CHYB) {
return analyza;
}
//pokud neni dopredna, vkladame do globalni hash tabulky a zaroven i do lokalni hash tabulky
if(init == true) {
htCompleteInsert(ptrhtGlobal,funkce, ID_FUNCTION, dtype, true);
htInsert(func[currFuncSize-1].table, funkce, dat, dtype, ID_FUNCTION);
}//pokud se jedna o doprednou deklaraci, ukladame jen do globalky hlavicku funkce
else if (init == false) {
htCompleteInsert(ptrhtGlobal,funkce, ID_FUNCTION, dtype, false);
//vlozeni navratove hodnoty do lokalni tabulky
}
if(token.stav == s_strednik) {
token = getNextToken();
if(token.stav == s_lex_error) {
return S_LEXIKALNI_CHYBA;
}
else {
//nastaveni ukazatele lokalni tabulky na vrchol zasobniku
ptrhtLocal = ptrStack->top->ptrht;
listIntrukci = &(ptrStack->top->field);
// nastaveni tabulky instrukci v mainu
byla_funkce = false;
//rekurzivni volani
return FUNKCE();
}
}
}
}
}
//pokud se nacte token, ktery podle pravidla tam nepatri, vrati se syntakticka chyba
return S_SYNTAKTICKA_CHYBA;
}
//FUNKCE->var DEKLARACE FUNKCE
else if(!strcmp(token.data, "var") && token.stav == s_klicove) {
token = getNextToken();
if(token.stav == s_lex_error) {
return S_LEXIKALNI_CHYBA;
}
//volani funkce na kontrolu a ukladani globalnich id
analyza = DEKLARACE();
if(analyza != S_BEZ_CHYB) {
return analyza;
}
return FUNKCE();
}
return S_SYNTAKTICKA_CHYBA;
}
