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 InitilizePidController(AbsPID * groups, int numberOfGroups,
float (*getPositionPtr)(int),
void (*setOutputPtr)(int, float),
int (*resetPositionPtr)(int, int),
void (*onPidConfigurePtr)(int),
PidLimitEvent* (*checkPIDLimitEventsPtr)(uint8_t group)) {
if (groups == 0 ||
getPositionPtr == 0 ||
setOutputPtr == 0 ||
resetPositionPtr == 0 ||
checkPIDLimitEventsPtr == 0 ||
onPidConfigurePtr == 0) {
println("Null pointer exception in PID Configure", ERROR_PRINT);
while (1);
}
pidGroupsInternal =groups;
number_of_pid_groups = numberOfGroups;
getPosition = getPositionPtr;
setOutputLocal = setOutputPtr;
resetPosition = resetPositionPtr;
onPidConfigureLocal = onPidConfigurePtr;
checkPIDLimitEvents = checkPIDLimitEventsPtr;
SetControllerMath(&RunAbstractPIDCalc);
int i;
for (i = 0; i < numberOfGroups; i++) {
int enabled = getPidGroupDataTable(i)->config.Enabled;
pidReset(i, 0);
//getPidGroupDataTable(i)->config.Enabled = enabled;
SetPIDEnabled(i, enabled);
//println_I("PID ");p_int_I(i);print_I(" enabled");
}
}
ControlHandle
pidInit(float gainP, float gainI, float gainD)
{
Pid * pid = malloc(sizeof(Pid));
pid->portal = NULL;
pid->gainP = gainP;
pid->gainI = gainI;
pid->gainD = gainD;
pidReset(pid);
return pid;
}
void InitPID(void){
BYTE i;
//WORD loop;
for (i=0;i<NUM_PID_GROUPS;i++){
//DyPID values
dyPid[i].inputChannel=DYPID_NON_USED;
dyPid[i].outputChannel=DYPID_NON_USED;
dyPid[i].inputMode=0;
dyPid[i].outputMode=0;
pidGroups[i].Enabled=FALSE;
pidGroups[i].channel = i;
vel[i].enabled=FALSE;
vel[i].K.P=.1;
limits[i].type=NO_LIMIT;
LoadPIDvals(&pidGroups[i],&dyPid[i]);
if( dyPid[i].inputChannel==DYPID_NON_USED ||
dyPid[i].outputChannel==DYPID_NON_USED ||
dyPid[i].outputChannel==dyPid[i].inputChannel)
{
dyPid[i].inputChannel=DYPID_NON_USED;
dyPid[i].outputChannel=DYPID_NON_USED;
WritePIDvalues(&pidGroups[i],&dyPid[i]);
}
force[i].MsTime=0;
force[i].setPoint=200;
}
InitilizePidController( pidGroups,
vel,
NUM_PID_GROUPS,
&getPositionMine,
&setOutputMine,
&resetPositionMine,
//&asyncCallback,
&onPidConfigureMine,
&checkPIDLimitEventsMine);
for (i=0;i<NUM_PID_GROUPS;i++){
printPIDvals(i);
if(pidGroups[i].Enabled){
initPIDChans(i);
println_I("Resetting PID channel from init");
int value = 0;
if(dyPid[i].inputMode == IS_ANALOG_IN)
value = 512;
pidReset(i,value);
}
}
}
inline void processFlightActionRequests()
{
if (g_flight_action_pkt_info->updated)
{
if(isSerialEnabled() != 0)
{
if (g_flight_action_pkt.action == MAV_ACTION_TOGGLE_ENGAGE)
{
if (g_flight_state_pkt.state == MAV_STATE_OFF)
{
g_flight_state_pkt.state = MAV_STATE_ENGAGING;
g_toggle_motors_start_time = g_timestamp;
}
else if (g_flight_state_pkt.state == MAV_STATE_IDLE)
{
g_flight_state_pkt.state = MAV_STATE_DISENGAGING;
g_toggle_motors_start_time = g_timestamp;
}
}
else if (g_flight_action_pkt.action == MAV_ACTION_ESTOP)
{
// estop
g_flight_state_pkt.state = MAV_STATE_ERROR;
}
else if (g_flight_action_pkt.action == MAV_ACTION_TAKEOFF)
{
if (g_flight_state_pkt.state == MAV_STATE_IDLE)
{
// takeoff
g_flight_state_pkt.state = MAV_STATE_FLYING;
// reset the PID controls
pidReset();
}
}
else if (g_flight_action_pkt.action == MAV_ACTION_LAND)
{
if (g_flight_state_pkt.state == MAV_STATE_FLYING)
{
// land
g_flight_state_pkt.state = MAV_STATE_LANDING;
//g_land_thrust = g_ctrl_cmd.cmd_thrust;
}
}
}
g_flight_action_pkt_info->updated = 0;
}
}
void HomeLinks(){
if(homingAllLinks){
if ( GetPIDCalibrateionState(linkToHWIndex(0))==CALIBRARTION_DONE&&
GetPIDCalibrateionState(linkToHWIndex(1))==CALIBRARTION_DONE&&
GetPIDCalibrateionState(linkToHWIndex(2))==CALIBRARTION_DONE
){
homingAllLinks = FALSE;
configured = TRUE;
println_W("All linkes reported in");
pidReset(hwMap.Extruder0.index,0);
int i;
float Alpha,Beta,Gama;
servostock_calcInverse(0, 0, getmaxZ(), &Alpha, &Beta, &Gama);
for(i=0;i<3;i++){
pidReset(linkToHWIndex(i), (Alpha+getRodLength()/3)/getLinkScale(i));
}
initializeCartesianController();
cancelPrint();
}
}
}
void attitudeControllerResetAllPID(void)
{
pidReset(&pidRoll);
pidReset(&pidPitch);
pidReset(&pidYaw);
pidReset(&pidRollRate);
pidReset(&pidPitchRate);
pidReset(&pidYawRate);
}
task flw_tsk_FeedForwardCntrl(){
pidReset(_fly.flyPID);
pidInit(_fly.flyPID, 0.6, 0.05, 0, 0, 999999);
int integralLimit;
if(_fly.flyPID.kI == 0){
integralLimit = 0;
} else{
integralLimit = 13.0 / _fly.flyPID.kI;
}
float output = 0;
float initTime = nPgmTime;
while(true){
fw_ButtonControl();
//
//we do not want to zero our error sum when we cross
if(abs(_fly.flyPID.errorSum) > integralLimit){
_fly.flyPID.errorSum = integralLimit * _fly.flyPID.errorSum/(abs(_fly.flyPID.errorSum));
}
_fly.currSpeed = FwCalculateSpeed();
float outVal = pidExecute(_fly.flyPID, _fly.setPoint - _fly.currSpeed);
float dTime = nPgmTime - initTime;
initTime = nPgmTime;
playTone(_fly.currSpeed/2,2);
output = _fly.pred + outVal;
if(output < 0){
output = 0;
}
if(output > 127){
output = 127;
}
if((_fly.currSpeed <= 50 && _fly.setPoint == 0) && vexRT[Btn7L] == 1){
output = -90;
_updateFlyWheel(output);
wait1Msec(4000);
_updateFlyWheel(0);
wait1Msec(10000);
}
_updateFlyWheel(output);
delay(FW_LOOP_SPEED);
}
}
//Gyro turn to target angle
void gyroTurn(float target)
{
if(abs(target) < 40)
pidInit(gyroPid, 3, 0, 0.15, 3, 1270);
bool atGyro = false;
long atTargetTime = nPgmTime;
long timer = nPgmTime;
pidReset(gyroPid);
gyroResetAngle();
while(!atGyro)
{
//Calculate the delta time from the last iteration of the loop
float dT = (float)(nPgmTime - timer)/1000;
//Calculate the current angle of the gyro
float angle = gyroAddAngle(dT);
//Reset loop timer
timer = nPgmTime;
//Calculate the output of the PID controller and output to drive motors
float error = (float)target - angle;
float driveOut = pidExecute(gyroPid, error);
driveL(-driveOut);
driveR(driveOut);
//Stop the turn function when the angle has been within 3 degrees of the desired angle for 350ms
if(abs(error) > 3)
atTargetTime = nPgmTime;
if(nPgmTime - atTargetTime > 350)
{
atGyro = true;
driveL(0);
driveR(0);
}
}
//Reinitialize the PID constants to their original values in case they were changed
pidInit(gyroPid, 2, 0, 0.15, 2, 1270);
}
boolean processPIDPost(BowlerPacket * Packet){
int chan, val;
float time;
switch (Packet->use.head.RPC){
case APID:
time = (float)get32bit(Packet,0);
uint8_t i=0;
for(i=0;i<Packet->use.data[4];i++){
SetPIDTimed(i,get32bit(Packet,5+(i*4)),time);
}
READY(Packet,zone,3);
break;
case _VPD:
chan = Packet->use.data[0];
val = get32bit(Packet,1);
time=get32bit(Packet,5);
StartPDVel(chan,val,time);
READY(Packet,zone,4);
break;
case _PID:
chan = Packet->use.data[0];
val = get32bit(Packet,1);
time=get32bit(Packet,5);
SetPIDTimed(chan,val,time);
READY(Packet,zone,5);
break;
case RPID:
chan = Packet->use.data[0];
b_println("Resetting PID channel from packet:",ERROR_PRINT);printBowlerPacketDEBUG(Packet,ERROR_PRINT);
pidReset(chan, get32bit(Packet,1));
READY(Packet,zone,6);
break;
default:
return false;
}
return true;
}
void runPidHysterisisCalibration(int group) {
if (!getPidGroupDataTable(group)->config.Enabled) {
println_E("Axis disabled for calibration #");
p_int_E(group);
getPidGroupDataTable(group)->config.Enabled = true;
}
getPidGroupDataTable(group)->config.lowerHistoresis = 0;
getPidGroupDataTable(group)->config.upperHistoresis = 0;
getPidGroupDataTable(group)->config.stop = 0;
// println_I("\tReset PID");
pidReset(group, 0); // Zero encoder reading
// println_I("\tDisable PID Output");
SetPIDEnabled(group, true);
SetPIDCalibrateionState(group, CALIBRARTION_hysteresis);
getPidGroupDataTable(group)->calibration.state = forward;
// println_I("\tSetting slow move");
setOutput(group, -1.0f);
getPidGroupDataTable(group)->timer.setPoint = 2000;
getPidGroupDataTable(group)->timer.MsTime = getMs();
}
void InitPID(void){
//println_I("Starting PID initialization ");
uint8_t i;
//uint16_t loop;
for (i=0;i<NUM_PID_GROUPS;i++){
//DyPID values
dyPid[i].inputChannel=DYPID_NON_USED;
dyPid[i].outputChannel=DYPID_NON_USED;
dyPid[i].inputMode=0;
dyPid[i].outputMode=0;
dyPid[i].flagValueSync=false;
pidGroups[i].config.tipsScale=1.0;
pidGroups[i].config.Enabled = false;
pidGroups[i].config.Async = 1;
pidGroups[i].config.IndexLatchValue = 0;
pidGroups[i].config.stopOnIndex = 0;
pidGroups[i].config.useIndexLatch = 0;
pidGroups[i].config.K.P = .1;
pidGroups[i].config.K.I = 0;
pidGroups[i].config.K.D = 0;
pidGroups[i].config.V.P = .1;
pidGroups[i].config.V.D = 0;
pidGroups[i].config.Polarity = 0;
pidGroups[i].config.stop = 0;
pidGroups[i].config.upperHistoresis = 0;
pidGroups[i].config.lowerHistoresis = 0;
pidGroups[i].config.offset = 0.0;
pidGroups[i].config.calibrationState = CALIBRARTION_DONE;
pidGroups[i].interpolate.set=0;
pidGroups[i].interpolate.setTime=0;
pidGroups[i].interpolate.start=0;
pidGroups[i].interpolate.startTime=0;
limits[i].type=NO_LIMIT;
LoadPIDvals(&pidGroups[i],&dyPid[i],i);
if( dyPid[i].inputChannel==DYPID_NON_USED ||
dyPid[i].outputChannel==DYPID_NON_USED ||
dyPid[i].outputChannel==dyPid[i].inputChannel||
pidGroups[i].config.Enabled !=true)
{
dyPid[i].inputChannel=DYPID_NON_USED;
dyPid[i].outputChannel=DYPID_NON_USED;
pidGroups[i].config.Enabled = false;
WritePIDvalues(&pidGroups[i],&dyPid[i],i);
}
force[i].MsTime=0;
force[i].setPoint=200;
}
InitilizePidController( pidGroups,
NUM_PID_GROUPS,
&getPositionMine,
&setOutputMine,
&resetPositionMine,
&onPidConfigureMine,
&checkPIDLimitEventsMine);
for (i=0;i<NUM_PID_GROUPS;i++){
//
if(pidGroups[i].config.Enabled){
println_W("PID ");p_int_W(i);
printPIDvals(i);
initPIDChans(i);
int value = getPositionMine(i);
pidGroups[i].CurrentState=value;
pidReset(i,value);
}
}
}
task autonomous()
{
clearTimer(T1);
pidInit(flywheel, 0.01, 0.03, 0.001, 3, 50);//p, i, d, epsilon, slew
unsigned long lastTime = nPgmTime;
resetMotorEncoder(flywheelLeftBot);
float rpm, lastRpm1, lastRpm2, lastRpm3, lastRpm4;
int counter = 0;
bool btn7DPressed;
while (true)
{
if(1==1)
{
dT = (float)(nPgmTime - lastTime)/1000;
lastTime = nPgmTime;
if(dT != 0)
{
rpm = 60.00*25*(((float)getMotorEncoder(flywheelLeftBot))/dT)/360;
}
else
{
rpm = 0;
}
resetMotorEncoder(flywheelLeftBot);
lastRpm4 = lastRpm3;
lastRpm3 = lastRpm2;
lastRpm2 = lastRpm1;
lastRpm1 = rpm;
rpmAvg = (rpm + lastRpm1 + lastRpm2 + lastRpm3 + lastRpm4) / 5;
if(flywheel.errorSum > 18000)
{
flywheel.errorSum = 18000;
}
if(rpmAvg >= sp && !set)
{
set = true;
}
if( rpm <= sp - 700)
{
set = false;
}
out = pidExecute(flywheel, sp-rpmAvg);
if(vexRT(Btn7U) == 1)
{
btn7DPressed = false;
pidReset(flywheel);
out = 0;
}
if(out < 1)
{
out = 1;
}
driveFlywheel(out);
}
// debugStrea
if(time1[T1] >= 7250 && time1[T1] <= 9000)
{
motor[conveyor] = 127;
motor[conveyor2] = 127;
}
}
/*dank memes
*/
/*
motor[flywheelLeftTop] = 63;
motor[flywheelLeftBot] = 63;
motor[flywheelRightTop] = 63;
motor[flywheelRightBot] = 63;
wait1Msec(2500);
motor[conveyor] = 127;
wait1Msec(1000);
motor[conveyor] = 0;
wait1Msec(1500);
motor[conveyor] = 127;
wait1Msec(1000);
motor[conveyor] = 0;
wait1Msec(1000);
motor[flywheelLeftTop] = 65;
motor[flywheelLeftBot] = 65;
motor[flywheelRightTop] = 65;
motor[flywheelRightBot] = 65;
motor[conveyor] = 127;
wait1Msec(1000);
motor[conveyor] = 0;
wait1Msec(1000);
motor[conveyor] = 127;
wait1Msec(1000);
motor[conveyor] = 0;
*/
//PROGRAMMING SKILLS
/* while(1==1)
{
motor[flywheelLeftTop] = 63;
motor[flywheelLeftBot] = 63;
motor[flywheelRightTop] = 63;
motor[flywheelRightBot] = 63;
motor[conveyor] = 127;
}*/
}
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;
}
}
void setFlyRpm(int rpm){
_fly.setPoint = rpm;
pidReset(_fly.flyPID);
}
void flightControl(void) {
// this should run at 500Hz
if ((micros() - elapsed_micros) >= (2000)) {
elapsed_micros = micros();
// flicker the leds to measure correct speed
GPIOA->ODR ^= 1<<15;
GPIOB->ODR ^= 1<<2;
// read gyro
gyro_read(&gyro_data);
// controls
/*
throttle = payload[0] * 0xff;
yaw = payload[1] * 0xff;
pitch = payload[3] * 0xff;
roll = payload[4] * 0xff;
// trims
yaw_trim = payload[2];
pitch_trim = payload[5];
roll_trim = payload[6];
*/
// convert channels for crazyflie PID:
#if 0
rollRateDesired = (float) (payload[4] - 128.0) * 256;
pitchRateDesired = (float) (payload[3] - 128.0) * 256;
yawRateDesired = (float) (payload[1] - 128.0) * 256;
#else
actuatorThrust = payload[0] * 256;
// Roll, aka aileron, float +- 50.0 in degrees
s32 f_roll = (payload[4] - 0x80) * 0x50 * FRAC_SCALE / (10000 / 400);
frac2float(f_roll, &rollRateDesired);
// Pitch, aka elevator, float +- 50.0 degrees
s32 f_pitch = (payload[3] - 0x80) * 0x50 * FRAC_SCALE / (10000 / 400);
frac2float(f_pitch, &pitchRateDesired);
// Thrust, aka throttle 0..65535, working range 5535..65535
// No space for overshoot here, hard limit Channel3 by -10000..10000
/*
s32 ch = payload[0] * 0xff;
if (ch < CHAN_MIN_VALUE) {
ch = CHAN_MIN_VALUE;
} else if (ch > CHAN_MAX_VALUE) {
ch = CHAN_MAX_VALUE;
}
actuatorThrust = ch*3L + 35535L;
*/
// Yaw, aka rudder, float +- 400.0 deg/s
s32 f_yaw = (payload[1] - 0x80) * 0x50 * FRAC_SCALE / (10000 / 400);
frac2float(f_yaw, &yawRateDesired);
#endif
// gyro.* == *rateActual == Data measured by IMU
// *rateDesired == Data from RX
controllerCorrectRatePID(-gyro_data.x, gyro_data.y, -gyro_data.z, rollRateDesired, pitchRateDesired, yawRateDesired);
//#define TUNE_ROLL
if (actuatorThrust > 0)
{
#if defined(TUNE_ROLL)
distributePower(actuatorThrust, rollOutput, 0, 0);
#elif defined(TUNE_PITCH)
distributePower(actuatorThrust, 0, pitchOutput, 0);
#elif defined(TUNE_YAW)
distributePower(actuatorThrust, 0, 0, -yawOutput);
#else
distributePower(actuatorThrust, rollOutput, pitchOutput, -yawOutput);
#endif
}
else
{
distributePower(0, 0, 0, 0);
pidReset(&pidRollRate);
pidReset(&pidPitchRate);
pidReset(&pidYawRate);
}
}
#if 0
m0_val = actuatorThrust;
m1_val = actuatorThrust;
m2_val = actuatorThrust;
m3_val = ((yawOutput * 1000) / 0xffff) + 1000;
TIM2->CCR4 = m0_val; // Motor "B"
TIM1->CCR1 = m1_val; // Motor "L"
TIM1->CCR4 = m2_val; // Motor "R"
TIM16->CCR1 = m3_val; // Motor "F"
#endif
}
uint8_t ZeroPID(uint8_t chan) {
//println("Resetting PID channel from zeroPID:",INFO_PRINT);
pidReset(chan, 0);
return true;
}
task usercontrol()
{
pidReset(flywheel);
debugStreamClear;
float sp = 2050;
// User control code here, inside the loop
int currentFlywheelSpeed;
// pidInit(flywheel, 0.10, 0.01, 0.00001, 3, 50);
pidInit(flywheel, 0.01, 0.03, 0.001, 3, 50);
unsigned long lastTime = nPgmTime;
resetMotorEncoder(flywheelLeftBot);
float rpm, lastRpm1, lastRpm2, lastRpm3, lastRpm4;
int counter = 0;
bool btn7DPressed;
while (true)
{
if(vexRT[btn7D] == 1)
{
btn7DPressed = true;
}
if(btn7DPressed == true)
{
dT = (float)(nPgmTime - lastTime)/1000;
lastTime = nPgmTime;
if(dT != 0)
{
rpm = 60.00*25*(((float)getMotorEncoder(flywheelLeftBot))/dT)/360;
}
else
{
rpm = 0;
}
resetMotorEncoder(flywheelLeftBot);
lastRpm4 = lastRpm3;
lastRpm3 = lastRpm2;
lastRpm2 = lastRpm1;
lastRpm1 = rpm;
rpmAvg = (rpm + lastRpm1 + lastRpm2 + lastRpm3 + lastRpm4) / 5;
if(flywheel.errorSum > 18000)
{
flywheel.errorSum = 18000;
}
if(rpmAvg >= sp && !set)
{
set = true;
}
if( rpm <= sp - 700)
{
set = false;
}
out = pidExecute(flywheel, sp-rpmAvg);
if(vexRT(Btn7U) == 1)
{
btn7DPressed = false;
pidReset(flywheel);
out = 0;
}
if(out < 1)
{
out = 1;
}
driveFlywheel(out);
}
/*
//motor[Motor1] = 127;
motor[flywheelLeftTop] = 68;
motor[flywheelLeftBot] = 68;
motor[flywheelRightTop] = 68;
motor[flywheelRightBot] = 68;
currentFlywheelSpeed = 68;
}
if(vexRT[Btn7L] == 1)
{
currentFlywheelSpeed -= 2;
motor[flywheelLeftTop] = currentFlywheelSpeed;
motor[flywheelLeftBot] = currentFlywheelSpeed;
motor[flywheelRightTop] = currentFlywheelSpeed;
motor[flywheelRightBot] = currentFlywheelSpeed;
wait1Msec(400);
}
if(vexRT[Btn7R] == 1)
{
currentFlywheelSpeed += 2;
motor[flywheelLeftTop] = currentFlywheelSpeed;
motor[flywheelLeftBot] = currentFlywheelSpeed;
motor[flywheelRightTop] = currentFlywheelSpeed;
motor[flywheelRightBot] = currentFlywheelSpeed;
wait1Msec(400);
}
if(vexRT[Btn7U] == 1)
{
// motor[Motor1] = 0;
motor[flywheelLeftTop] = 0;
motor[flywheelLeftBot] = 0;
motor[flywheelRightTop] = 0;
motor[flywheelRightBot] = 0;
}
if(vexRT[Btn8L] == 1)
{
motor[flywheelLeftTop] = 48;
motor[flywheelLeftBot] = 48;
motor[flywheelRightTop] = 48;
motor[flywheelRightBot] = 48;
currentFlywheelSpeed = 48;
}*/
if(vexRT[Btn8D] == 1)
{
motor[conveyor] = 127;
motor[conveyor2] = 127;
}
if(vexRT[Btn8R] == 1)
{
motor[conveyor] = 0;
motor[conveyor2] = 0;
}
if(vexRT[Btn8U] == 1)
{
motor[conveyor] = -127;
motor[conveyor2] = -127;
}
/* motor[driveFrontRight] = vexRT[Ch1] + vexRT[Ch4];
motor[driveFrontLeft] = vexRT[Ch1] - vexRT[Ch4];
motor[driveBackRight] = vexRT[Ch1] - vexRT[Ch4];
motor[driveBackLeft] = vexRT[Ch1] + vexRT[Ch4];*/
motor[driveFrontRight] = vexRT[Ch2];
motor[driveBackRight] = vexRT[Ch2];
motor[driveFrontLeft] = vexRT[Ch3];
motor[driveBackLeft] = vexRT[Ch3];
while(vexRT(Btn5U)) //Left strafe
{
motor[driveFrontRight] = 127;
motor[driveBackRight] = -127;
motor[driveFrontLeft] = -127;
motor[driveBackLeft] = 127;
}
while(vexRT(Btn6U))//RIGHT STRAFE
{
motor[driveFrontRight] = -127;
motor[driveBackRight] = 127;
motor[driveFrontLeft] = 127;
motor[driveBackLeft] = -127;
}
/*
if(vexRT[Ch2] > 2)
{
motor[driveFrontRight] = 127;
motor[driveFrontLeft] = -127;
motor[driveBackRight] = 127;
motor[driveBackLeft] = -127;
}
if(vexRT[Ch2] < -2)
{
motor[driveFrontRight] = -127;
motor[driveFrontLeft] = 127;
motor[driveBackRight] = -127;
motor[driveBackLeft] = 127;
}
*/
writeDebugStreamLine("%f", rpm);
wait1Msec(20);
}
}
