boolean processPIDGet(BowlerPacket * Packet){
int i;
switch (Packet->use.head.RPC){
case APID:
Packet->use.head.DataLegnth=5;
Packet->use.data[0]=getNumberOfPidChannels();
for(i=0;i<getNumberOfPidChannels();i++){
set32bit(Packet,GetPIDPosition(i),1+(i*4));
Packet->use.head.DataLegnth+=4;
}
Packet->use.head.Method=BOWLER_POST;
break;
case _PID:
set32bit(Packet,GetPIDPosition(Packet->use.data[0]),1);
Packet->use.head.DataLegnth=4+1+4;
Packet->use.head.Method=BOWLER_POST;
break;
case CPID:
GetConfigPID(Packet);
break;
case CPDV:
GetConfigPDVelocity(Packet);
break;
case GPDC:
set32bit(Packet,getNumberOfPidChannels(),0);
Packet->use.head.DataLegnth=4+4;
Packet->use.head.Method=BOWLER_POST;
break;
default:
return false;
}
return true;
}
uint8_t SetPIDTimed(uint8_t chan, float val, float ms) {
//println_I("@#@# PID channel Set chan=");p_int_I(chan);print_I(" setpoint=");p_int_I(val);print_I(" time=");p_fl_I(ms);
if (chan >= getNumberOfPidChannels())
return false;
getPidVelocityDataTable(chan)->enabled = false;
return SetPIDTimedPointer(getPidGroupDataTable(chan), val, GetPIDPosition(chan), ms);
}
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;
}
void StartPDVel(uint8_t chan,float unitsPerSeCond,float ms){
if(ms<.1){
//println_I("Starting Velocity");
getPidVelocityDataTable(chan)->enabled=true;
getPidGroupDataTable(chan)->config.Enabled=false;
getPidVelocityDataTable(chan)->lastPosition=GetPIDPosition(chan);
getPidVelocityDataTable(chan)->lastTime=getMs();
getPidVelocityDataTable(chan)->unitsPerSeCond=unitsPerSeCond;
getPidVelocityDataTable(chan)->currentOutputVel =0;
}else{
//println_I("Starting Velocity Timed");
float seConds = ms/1000;
int32_t dist = (int32_t) unitsPerSeCond*(int32_t) seConds;
int32_t delt = ((int32_t) (GetPIDPosition(chan))-dist);
SetPIDTimed(chan, delt, ms);
}
}
void startHomingLink(int group, PidCalibrationType type, float homedValue) {
float speed = 20.0;
if (type == CALIBRARTION_home_up)
speed *= 1.0;
else if (type == CALIBRARTION_home_down)
speed *= -1.0;
else {
println_E("Invalid homing type");
return;
}
getPidGroupDataTable(group)->config.tipsScale = 1;
SetPIDCalibrateionState(group, type);
setOutput(group, speed);
getPidGroupDataTable(group)->timer.MsTime = getMs();
getPidGroupDataTable(group)->timer.setPoint = 1000;
getPidGroupDataTable(group)->homing.homingStallBound = 20;
getPidGroupDataTable(group)->homing.previousValue = GetPIDPosition(group);
getPidGroupDataTable(group)->homing.lastTime = getMs();
getPidGroupDataTable(group)->homing.homedValue = homedValue;
SetPIDEnabled(group,true);
}
float getLinkAngle(int index){
int localIndex=linkToHWIndex(index);
return GetPIDPosition(localIndex)*getLinkScale(index);
}
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;
}
}
