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);
}
}
CAL_STATE pidHysterisis(int group) {
if (RunEvery(&getPidGroupDataTable(group)->timer) > 0) {
Print_Level l = getPrintLevel();
//setPrintLevelInfoPrint();
float boundVal = 150.0;
float extr = GetPIDPosition(group);
if (bound(0, extr, boundVal, boundVal)) {// check to see if the encoder has moved
//we have not moved
// println_I("NOT moved ");p_fl_I(extr);
if (getPidGroupDataTable(group)->calibration.state == forward) {
incrementHistoresis(group);
} else if (getPidGroupDataTable(group)->calibration.state == backward) {
decrementHistoresis(group);
}
int historesisBound = 25;
if (getPidGroupDataTable(group)->config.lowerHistoresis < (-historesisBound) &&
getPidGroupDataTable(group)->calibration.state == backward) {
println_E("Backward Motor seems damaged, more then counts of historesis #");
p_int_I(group);
getPidGroupDataTable(group)->calibration.state = forward;
}
if (getPidGroupDataTable(group)->config.upperHistoresis > (historesisBound) &&
getPidGroupDataTable(group)->calibration.state == forward) {
println_E("Forward Motor seems damaged, more then counts of historesis #");
p_int_I(group);
getPidGroupDataTable(group)->calibration.state = done;
}
} else {
pidReset(group, 0);
setOutput(group, 0);
println_E("Moved ");
p_fl_E(extr);
if (getPidGroupDataTable(group)->calibration.state == forward) {
println_I("Backward Calibrated for link# ");
p_int_I(group);
getPidGroupDataTable(group)->calibration.state = backward;
} else {
println_I("Calibration done for link# ");
p_int_I(group);
getPidGroupDataTable(group)->calibration.state = done;
float offset = .9;
getPidGroupDataTable(group)->config.lowerHistoresis *= offset;
getPidGroupDataTable(group)->config.upperHistoresis *= offset;
calcCenter(group);
}
}
if (getPidGroupDataTable(group)->calibration.state == forward) {
setOutput(group, 1.0f);
} else if (getPidGroupDataTable(group)->calibration.state == backward) {
setOutput(group, -1.0f);
}
setPrintLevel(l);
}
if (getPidGroupDataTable(group)->calibration.state == done)
SetPIDCalibrateionState(group, CALIBRARTION_DONE);
return getPidGroupDataTable(group)->calibration.state;
}
uint8_t Bowler_Server_Local(BowlerPacket * Packet){
Print_Level l = getPrintLevel();
//setPrintLevelNoPrint();
if (GetBowlerPacket_arch(Packet)){
//setLed(1,1,1);
if(Packet->use.head.RPC != _PNG){
println_I("Got:");printPacket(Packet,INFO_PRINT);
}
if ( (CheckAddress(MyMAC.v,Packet->use.head.MAC.v) == true) || ((CheckAddress((uint8_t *)Broadcast.v,(uint8_t *)Packet->use.head.MAC.v) == true) )) {
float start=getMs();
Process_Self_Packet(Packet);
if(getMs()-start>5){
println_E("Process too long: ");p_fl_E(getMs()-start);
}
for (i=0;i<6;i++){
Packet->use.head.MAC.v[i]=MyMAC.v[i];
}
SetCRC(Packet);
start=getMs();
PutBowlerPacket(Packet);
if(getMs()-start>5){
println_E("Return too long: ");p_fl_E(getMs()-start);
}
if(Packet->use.head.RPC != _PNG){
println_I("Response:");printPacket(Packet,INFO_PRINT);
}
}else{
//println_I("Packet not addressed to me: ");printByteArray(Packet->use.head.MAC.v,6); print_I(" is not mine: ");printByteArray(MyMAC.v,6);
}
//setLed(0,0,1);
setPrintLevel(l);
return true;
}//Have a packet
setPrintLevel(l);
return false;
}
void bowlerSystem(){
Bowler_Server_Local(&MyPacket);
float diff = RunEvery(&pid);
if(diff>0){
RunNamespaceAsync(&MyPacket,&asyncCallback);
if(diff>pid.setPoint){
println_E("Time diff ran over! ");p_fl_E(diff);
pid.MsTime=getMs();
}
cartesianAsync();
}
}
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
}
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;
}
}