int main(){
//setPrintLevelInfoPrint();
setPrintLevelWarningPrint();
//setPrintLevelNoPrint();
hardwareInit();
RunEveryData loop = {0.0,2000.0};
while(1){
if (_RF5==1){
setPrintLevelErrorPrint();
p_int_E(0);print_E(" Reset Button Pressed from loop");
SetColor(1,1,1);
U1CON = 0x0000;
DelayMs(100);
Reset();
}
if(RunEvery(&loop)>0){
// clearPrint();
// printCartesianData();
}
if( printCalibrations == false &&
GetPIDCalibrateionState(linkToHWIndex(0))==CALIBRARTION_DONE&&
GetPIDCalibrateionState(linkToHWIndex(1))==CALIBRARTION_DONE&&
GetPIDCalibrateionState(linkToHWIndex(2))==CALIBRARTION_DONE
){
printCalibrations = true;
int index=0;
for(index=0;index<3;index++){
int group = linkToHWIndex(index);
println_E("For Axis ");p_int_E(group);
print_E(" upper: ");p_int_E(getPidGroupDataTable(group)->config.upperHistoresis);
print_E(" lower: ");p_int_E(getPidGroupDataTable(group)->config.lowerHistoresis);
print_E(" stop: ");p_int_E(getPidGroupDataTable(group)->config.stop);
}
startHomingLinks();
//Print_Level l= getPrintLevel();
//setPrintLevelInfoPrint();
printCartesianData();
int i;
for(i=0;i<numPidMotors;i++){
printPIDvals(i);
}
//setPrintLevel(l);
}
bowlerSystem();
}
}
uint8_t SetCoProcMode(uint8_t pin, uint8_t mode) {
if (GetChannelMode(pin) == mode)
return true;
println_E("Setting Mode: ");print_E(" on: ");p_int_E(pin);printMode(mode,ERROR_PRINT);
//getBcsIoDataTable(pin)->PIN.currentChannelMode = mode;
SetChannelModeDataTable(pin,mode);
down[pin].changeMode = true;
return false;
}
void SetNewConfigurationDataTable(uint8_t pin, int32_t value) {
println_E("Loading to datatable ");
p_int_E(pin);
print_E(" to ");
p_int_E(value);
if (down[pin].currentConfiguration != value) {
down[pin].changeConfiguration = true;
down[pin].currentConfiguration = value;
}
}
BYTE setXYZ(float x, float y, float z,float ms){
updateCurrentPositions();
float t0=0,t1=0,t2=0;
if(hwMap.iK_callback( x, y, z, &t0, &t1, &t2)==0){
println_I("New target angles t1=");p_fl_I(t0);print_I(" t2=");p_fl_I(t1);print_I(" t3=");p_fl_I(t2);
setLinkAngle(0,t0,ms);
setLinkAngle(1,t1,ms);
setLinkAngle(2,t2,ms);
}else{
println_E("Interpolate failed, can't reach: x=");p_fl_E(x);print_E(" y=");p_fl_E(y);print_E(" z=");p_fl_E(z);
}
}
int servostock_calcInverse(float X, float Y, float Z, float *Alpha, float *Beta, float *Gamma){
float L = getRodLength();
float R = getBaseRadius()-getEndEffectorRadius();
float Lsqr=L*L;
float maxRad=sqrt((X*X)+(Y*Y));
//#warning "Z is not used yet"
if((maxRad>(L-R))|| (Z<getminZ())||(Z>(getmaxZ()+L))){
println_E("Outside of workspace x=");p_fl_E(X);print_E(" y=");p_fl_E(Y);print_E(" z=");p_fl_E(Z);print_E(" Bound radius=");p_fl_E((maxRad));
//printf("\r\nOutside of workspace x= %g y=%g z=%g Bound = %g",X,Y,Z,maxRad);
return 1;//This is ourside the reachable work area
}
float SIN_60 = 0.8660254037844386;
float COS_60 = 0.5;
// Values are in mm, Alpha, Beta, Gamma starts at 0 at the base platform.
Alpha[0] = sqrt(Lsqr - (0 - X)*(0 - X) - (R - Y)*(R - Y))+Z;
Beta[0] = sqrt(Lsqr - (-SIN_60*R - X)*(-SIN_60*R - X) - (-COS_60*R - Y)*(-COS_60*R - Y))+Z;
Gamma[0] = sqrt(Lsqr - (SIN_60*R - X)*(SIN_60*R - X) - (-COS_60*R - Y)*(-COS_60*R - Y))+Z;
if( abs(Alpha[0]-Beta[0])>L||
abs(Alpha[0]-Gamma[0])>L||
abs(Beta[0]-Alpha[0])>L||
abs(Beta[0]-Gamma[0])>L||
abs(Gamma[0]-Alpha[0])>L||
abs(Gamma[0]-Beta[0])>L){
println_E("Outside of workspace x=");p_fl_E(X);print_E(" y=");p_fl_E(Y);print_E(" z=");p_fl_E(Z);print_E(" Bound radius=");p_fl_E((maxRad));
println_E("Alpha=");p_fl_E(Alpha[0]);
print_E(" Beta=");p_fl_E(Beta[0]);
print_E(" Gama=");p_fl_E(Gamma[0]);
return 1;//This is ourside the reachable work area
}
return 0;//SUCCESS
}
Elem & Machine::execute(std::ostream &out)
{
Instruction *command;
std::shared_ptr<Elem> command_ptr;
Elem *ADD(new Instruction("ADD"));
Elem *MUL(new Instruction("MUL"));
Elem *SUB(new Instruction("SUB"));
Elem *DIV(new Instruction("DIV"));
Elem *REM(new Instruction("REM"));
Elem *EQ(new Instruction("EQ"));
Elem *LEQ(new Instruction("LEQ"));
Elem *SEL(new Instruction("SEL"));
Elem *LD(new Instruction("LD"));
Elem *LDC(new Instruction("LDC"));
Elem *LDF(new Instruction("LDF"));
Elem *CAR(new Instruction("CAR"));
Elem *CDR(new Instruction("CDR"));
Elem *CONS(new Instruction("CONS"));
Elem *NIL(new Instruction("NIL"));
Elem *DUM(new Instruction("DUM"));
Elem *AP(new Instruction("AP"));
Elem *RAP(new Instruction("RAP"));
Elem *RTN(new Instruction("RTN"));
Elem *JOIN(new Instruction("JOIN"));
Elem *STOP(new Instruction("STOP"));
while (!C->empty())
{
if (out != 0x0)
{
print_S(out);
print_E(out);
print_C(out);
out << std::endl;
}
command_ptr = C->pop_ret();
command = dynamic_cast<Instruction*>(&*command_ptr);
if (command == nullptr) throw Exception("Execute", "FatalError");
if (*command == *ADD) this->ADD();
else if (*command == *MUL) this->MUL();
else if (*command == *SUB) this->SUB();
else if (*command == *DIV) this->DIV();
else if (*command == *REM) this->REM();
else if (*command == *EQ) this->EQ();
else if (*command == *LEQ) this->LEQ();
else if (*command == *SEL) this->SEL();
else if (*command == *LD) this->LD();
else if (*command == *LDC) this->LDC();
else if (*command == *LDF) this->LDF();
else if (*command == *CAR) this->CAR();
else if (*command == *CDR) this->CDR();
else if (*command == *CONS) this->CONS();
else if (*command == *NIL) this->NIL();
else if (*command == *DUM) this->DUM();
else if (*command == *AP) this->AP();
else if (*command == *RAP) this->RAP();
else if (*command == *RTN) this->RTN();
else if (*command == *JOIN) this->JOIN();
else if (*command == *STOP) { return (*(this->STOP()));}
else throw Exception("Execute", "Expected 'instruction' but greeted constant.");
}
throw Exception("Execute", "FatalError");
}
boolean pushAsyncReady( uint8_t pin){
if(!IsAsync(pin)){
//println_I("No asyinc on pin ");p_int_I(pin);print_I(" Mode 0x");prHEX8(GetChannelMode(pin),INFO_PRINT);
return false;
}
int32_t last;
int32_t aval;
int32_t db;
//int i=pin;
//EndCritical();
// println_I("Checking timer \nMsTime: ");p_fl_I(tRef->MsTime);
// print_I(" \nSetpoint: ");p_fl_I(tRef->setPoint);
// print_I(" \nCurrentTime: ");p_fl_I(getMs());
float timeout = RunEvery(getPinsScheduler( pin));
// print_I(" \nTimeout: ");p_fl_I(timeout);
if(GetChannelMode(pin)== IS_SERVO){
aval = GetServoPos(pin);
}else{
aval = getDataTableCurrentValue(pin);
}
if(timeout !=0){
// println_I("Time to do something");
switch(getBcsIoDataTable(pin)->PIN.asyncDataType){
case AUTOSAMP:
// println_I("Auto samp ");p_int_I(pin);
getBcsIoDataTable(pin)->PIN.asyncDataPreviousVal = aval;
return true;
case NOTEQUAL:
//
if(aval != getBcsIoDataTable(pin)->PIN.asyncDataPreviousVal){
//println_I("not equ ");p_int_I(pin);
//print_I("\t");
//p_int_I(getBcsIoDataTable(pin)->PIN.asyncDataPreviousVal);
//print_I("\t");
//p_int_I(getBcsIoDataTable(pin)->PIN.currentValue);
getBcsIoDataTable(pin)->PIN.asyncDataPreviousVal = aval;
return true;
}
break;
case DEADBAND:
last = getBcsIoDataTable(pin)->PIN.asyncDataPreviousVal;
db = getBcsIoDataTable(pin)->PIN.asyncDatadeadBandval;
if(!bound(last,aval,db,db)){
//println_I("deadband");p_int_I(pin);
getBcsIoDataTable(pin)->PIN.asyncDataPreviousVal=aval;
return true;
}
break;
case THRESHHOLD:
//println_I("treshhold");p_int_I(pin);
last = getBcsIoDataTable(pin)->PIN.asyncDataPreviousVal;
db = getBcsIoDataTable(pin)->PIN.asyncDatathreshholdval;
if(getBcsIoDataTable(pin)->PIN.asyncDatathreshholdedge == ASYN_RISING || getBcsIoDataTable(pin)->PIN.asyncDatathreshholdedge == ASYN_BOTH){
if(last<= db && aval>db){
getBcsIoDataTable(pin)->PIN.asyncDataPreviousVal = aval;
return true;
}
}
if(getBcsIoDataTable(pin)->PIN.asyncDatathreshholdedge == ASYN_FALLING|| getBcsIoDataTable(pin)->PIN.asyncDatathreshholdedge == ASYN_BOTH){
if(last> db && aval<=db){
getBcsIoDataTable(pin)->PIN.asyncDataPreviousVal = aval;
return true;
}
}
break;
default:
println_E("\nNo type defined!! chan: ");p_int_E(pin);
print_E(" mode: ");printMode(pin,GetChannelMode(pin),ERROR_PRINT);
print_E(" type: ");printAsyncType(pin,ERROR_PRINT);
startAdvancedAsyncDefault(pin);
break;
}
}else{
// println_I("Nothing to do, returning");
}
return false;
}
void checkLinkHomingStatus(int group) {
if (!(GetPIDCalibrateionState(group) == CALIBRARTION_home_down ||
GetPIDCalibrateionState(group) == CALIBRARTION_home_up ||
GetPIDCalibrateionState(group) == CALIBRARTION_home_velocity
)
) {
return; //Calibration is not running
}
float current = GetPIDPosition(group);
float currentTime = getMs();
if (RunEvery(&getPidGroupDataTable(group)->timer) > 0) {
//println_W("Check Homing ");
if (GetPIDCalibrateionState(group) != CALIBRARTION_home_velocity) {
float boundVal = getPidGroupDataTable(group)->homing.homingStallBound;
if (bound(getPidGroupDataTable(group)->homing.previousValue,
current,
boundVal,
boundVal
)
) {
pidReset(group, getPidGroupDataTable(group)->homing.homedValue);
//after reset the current value will have changed
current = GetPIDPosition(group);
getPidGroupDataTable(group)->config.tipsScale = 1;
println_W("Homing Velocity for group ");
p_int_W(group);
print_W(", Resetting position to: ");
p_fl_W(getPidGroupDataTable(group)->homing.homedValue);
print_W(" current ");
p_fl_W(current);
float speed = -20.0;
if (GetPIDCalibrateionState(group) == CALIBRARTION_home_up)
speed *= 1.0;
else if (GetPIDCalibrateionState(group) == CALIBRARTION_home_down)
speed *= -1.0;
else {
println_E("Invalid homing type");
return;
}
Print_Level l = getPrintLevel();
//setPrintLevelInfoPrint();
setOutput(group, speed);
setPrintLevel(l);
getPidGroupDataTable(group)->timer.MsTime = getMs();
getPidGroupDataTable(group)->timer.setPoint = 2000;
SetPIDCalibrateionState(group, CALIBRARTION_home_velocity);
getPidGroupDataTable(group)->homing.lastTime = currentTime;
}
} else {
current = GetPIDPosition(group);
float posDiff = current - getPidGroupDataTable(group)->homing.previousValue; //ticks
float timeDiff = (currentTime - getPidGroupDataTable(group)->homing.lastTime) / 1000.0; //
float tps = (posDiff / timeDiff);
getPidGroupDataTable(group)->config.tipsScale = 20 / tps;
println_E("New scale factor: ");
p_fl_E(getPidGroupDataTable(group)->config.tipsScale);
print_E(" speed ");
p_fl_E(tps);
print_E(" on ");
p_int_E(group);
print_E(" Position difference ");
p_fl_E(posDiff);
print_E(" time difference ");
p_fl_E(timeDiff);
OnPidConfigure(group);
SetPIDCalibrateionState(group, CALIBRARTION_DONE);
}
getPidGroupDataTable(group)->homing.previousValue = current;
}
}
