// verified
// read the shared variable steering
public void setMotorPower(float avg_pwr)
{
// limit steering: [-50, 50]
float new_steering = steering;
if (steering > 50)
{
new_steering = 50;
}
if (steering < -50)
{
new_steering = -50;
}
float extra_pwr = 0;
if (new_steering == 0)
{
int sync_0 = 0;
if (old_steering != 0)
{
sync_0 = ev3.getMotorDDegree() - ev3.getMotorADegree();
}
extra_pwr = (ev3.getMotorDDegree() - ev3.getMotorADegree() - sync_0) * 0.05f;
}
else
{
extra_pwr = new_steering * (-0.5f);
}
float pwr_c = avg_pwr - extra_pwr;
float pwr_b = avg_pwr + extra_pwr;
old_steering = new_steering;
float speedA = (pwr_b * 0.021f / radius);
float speedD = (pwr_c * 0.021f / radius);
// ev3.setPowerMotorA ((int)speedA);
// ev3.setPowerMotorD ((int)speedD);
ev3.onMotorA((int)speedA);
ev3.onMotorD((int)speedD);
//Console.WriteLine (speedA.ToString(FORMAT) + "\t" + speedD.ToString(FORMAT)
// + "\t" + extra_pwr.ToString(FORMAT) + "\t" + pwr_b.ToString(FORMAT) + "\t" + pwr_c.ToString(FORMAT));
}
public void pidControl()
{
Console.WriteLine("iter\tnormVal\terror\tpid_val\tpower");
// while (!Button.ESCAPE.isDown())
// while (true)
while (iter++ < max_iter)
{
// int normVal = ls.readNormalizedValue();
int normVal = ev3.getAngularVelocity();
// Proportional Error:
int error = normVal - offset;
// Adjust far and near light readings:
// if (error < 0) error = (int)(error * 1.8F);
// Integral Error:
int_error = ((int_error + error) * 2) / 3;
// Derivative Error:
int deriv_error = error - prev_error;
prev_error = error;
int pid_val = (int)(KP * error + KI * int_error + KD * deriv_error) / SCALE_PID;
if (pid_val > 100)
{
pid_val = 100;
}
if (pid_val < -100)
{
pid_val = -100;
}
// Power derived from PID value:
int power = Math.Abs(pid_val);
power = SPEED + (power * NORM_POWER) / SCALE_POWER; // NORMALIZE POWER
Console.Write(iter + "\t" + normVal + "\t" + error + "\t"
+ pid_val + "\t" + power + "\t");
if (pid_val >= 0)
{
Console.WriteLine("Forward");
ev3.onMotorA(power);
ev3.onMotorD(power);
// MotorPort.B.controlMotor(power, BasicMotorPort.FORWARD);
// MotorPort.C.controlMotor(power, BasicMotorPort.FORWARD);
}
else
{
Console.WriteLine("Backward");
ev3.onMotorA(-1 * power);
ev3.onMotorD(-1 * power);
// MotorPort.B.controlMotor(power, BasicMotorPort.BACKWARD);
// MotorPort.C.controlMotor(power, BasicMotorPort.BACKWARD);
}
// Thread.Sleep (SLEEP);
}
}
private void balance()
{
Console.WriteLine("start balancing ...");
starting_balancing_task = true;
///////////////////////////
//ADVANCED USER CONTROL
///////////////////////////
/*
* The following values can be modified for advanced control. Many users will not use these features,
* which is why I do not require these to be set in the main program. You can just modify them here.
* if you are unsure about what they do, just leave them at the default value.
*/
// Set gearing down ratio between motor and wheels (e.g. 5x slow down: 40z / 8z = 5)
// The default is 1, no gearing down.
gear_down_ratio = 1;
// Set the time each loop cycle should last. You can set it up to 0.03 seconds or even higher, if you really
// need to. If you add code to the control loop below (such as to read another sensor), make sure that
// all code can run in under dt seconds. If it takes more time, set dt to a higher value.
// Default is 0.010 seconds (10 miliseconds).
dt = 0.010f;
// Customize PID constants. These variables are global, so you can optionally dynamically change them in your main task.
gn_dth_dt = 0.23f;
gn_th = 25.00f;
gn_y = 272.8f;
gn_dy_dt = 24.6f;
kp = 0.0336f;
ki = 0.2688f;
kd = 0.000504f;
///////////////////////////
//END ADVANCED USER CONTROL
///////////////////////////
//MOTOR SETUP
int motorB = 1;
int motorC = 2;
int mtrNoReg = 0;
int[] nMotorPIDSpeedCtrl = new int[4];
nMotorPIDSpeedCtrl[motorB] = mtrNoReg;
nMotorPIDSpeedCtrl[motorC] = mtrNoReg;
int[] nMotorEncoder = new int[4];
nMotorEncoder[motorC] = 0;
nMotorEncoder[motorB] = 0;
int[] motor = new int[4];
ev3.resetMotorATachoCount();
ev3.resetMotorDTachoCount();
// Sensor setup
int Gyro = 2;
int[] SensorType = new int[5];
// SensorType[Gyro] = ev3.getAngularVelocity();
int nSensorsDefined = 0;
mean_reading = 0;
/*
#ifdef HiTechnic_Gyro
* SensorType[Gyro] = sensorRawValue;
* // The following sets the average HiTechnic sensor value. If you know the average, you can avoid the calibration
* // next time like so: mean_reading = 593.82; (if that's your sensor average).
* mean_reading = calibrate_hitechnic();
* nSensorsDefined++;
#endif
#ifdef MindSensors_IMU
* int ux,uy,uz; // Mindsensors Sensor Measurement
* mean_reading = 0;
* SensorType[Gyro] = sensorI2CCustomFastSkipStates;
* wait1Msec(500);
* MSIMUsetGyroFilter(Gyro, 0x00);
* wait1Msec(1000);
* nSensorsDefined++;
#endif
* if(nSensorsDefined != 1){
* nxtDisplayTextLine(0,"Check Sensor");
* nxtDisplayTextLine(1,"definition!");
* wait1Msec(5000);StopAllTasks();
* }
*/
//MATH CONSTANTS
const float radius = wheel_diameter / 1000;
const float degtorad = (float)Math.PI / 180;
//SETUP VARIABLES FOR CALCULATIONS
float u = 0; // Sensor Measurement (raw)
float th = 0, //Theta // Angle of robot (degree)
dth_dt = 0; //dTheta/dt // Angular velocity of robot (degree/sec)
float e = 0, //Error // Sum of four states to be kept zero: th, dth_dt, y, dy_dt.
de_dt = 0, //dError/dt // Change of above error
_edt = 0, //Integral Error // Accumulated error in time
e_prev = 0; //Previous Error/ Error found in previous loop cycle
float pid = 0; // SUM OF PID CALCULATION
float y = 0, //y // Measured Motor position (degrees)
dy_dt = 0, //dy/dt // Measured motor velocity (degrees/sec)
v = 0, //velocity // Desired motor velocity (degrees/sec)
y_ref = 0; //reference pos // Desired motor position (degrees)
int motorpower = 0, // Power ultimately applied to motors
last_steering = 0, // Steering value in previous cycle
straight = 0, // Average motor position for synchronizing
d_pwr = 0; // Change in power required for synchronizing
const int n_max = 7; // Number of measurement used for floating motor speed average
int n = 0, n_comp = 0; // Intermediate variables needed to compute measured motor speed
int[] encoder = new int[n_max]; // Array containing last n_max motor positions
// memset(&encoder[0],0,sizeof(encoder));
starting_balancing_task = false; // We're done configuring. Main task now resumes.
// ClearTimer(T4); // This timer is used in the driver. Do not use it for other purposes!
Console.WriteLine("iter\tu\tpid\tth\tmotorpower\td_pwr\tmotorB\tmotorC");
int iter = 0;
// while(true)
while (iter++ < max_iter)
{
//READ GYRO SENSOR
u = ev3.getAngularVelocity();
Thread.Sleep(2);
u = ev3.getAngularVelocity();
/*
#ifdef HiTechnic_Gyro
* u = SensorRaw[Gyro];wait1Msec(2);
* u = u+SensorRaw[Gyro];
#endif
#ifdef MindSensors_IMU
* MSIMUreadGyroAxes(Gyro, ux, uy, uz);
* u = uz*0.0210;
#endif
* ////////////
*/
//COMPUTE GYRO ANGULAR VELOCITY AND ESTIMATE ANGLE
dth_dt = u / 2 - mean_reading;
mean_reading = mean_reading * 0.999f + (0.001f * (dth_dt + mean_reading));
th = th + dth_dt * dt;
//ADJUST REFERENCE POSITION ON SPEED AND ACCELERATION
if (v < speed * 10)
{
v = v + acceleration * 10 * dt;
}
else if (v > speed * 10)
{
v = v - acceleration * 10 * dt;
}
y_ref = y_ref + v * dt;
//COMPUTE MOTOR ENCODER POSITION AND SPEED
nMotorEncoder [motorB] = ev3.getMotorADegree();
nMotorEncoder [motorC] = ev3.getMotorDDegree();
n++; if (n == n_max)
{
n = 0;
}
encoder[n] = nMotorEncoder[motorB] + nMotorEncoder[motorC] + (int)y_ref;
n_comp = n + 1; if (n_comp == n_max)
{
n_comp = 0;
}
y = encoder[n] * degtorad * radius / gear_down_ratio;
dy_dt = (encoder[n] - encoder[n_comp]) / (dt * (n_max - 1)) * degtorad * radius / gear_down_ratio;
//COMPUTE COMBINED ERROR AND PID VALUES
e = gn_th * th + gn_dth_dt * dth_dt + gn_y * y + gn_dy_dt * dy_dt;
de_dt = (e - e_prev) / dt;
_edt = _edt + e * dt;
e_prev = e;
pid = (kp * e + ki * _edt + kd * de_dt) / radius * gear_down_ratio;
//ADJUST MOTOR SPEED TO STEERING AND SYNCHING
if (steering == 0)
{
if (last_steering != 0)
{
straight = nMotorEncoder[motorC] - nMotorEncoder[motorB];
}
d_pwr = (int)((nMotorEncoder[motorC] - nMotorEncoder[motorB] - straight) / (radius * 10 / gear_down_ratio));
}
else
{
d_pwr = (int)(steering / (radius * 10 / gear_down_ratio));
}
last_steering = steering;
//CONTROL MOTOR POWER AND STEERING
motorpower = (int)pid;
motor[motorB] = motorpower + d_pwr;
motor[motorC] = motorpower + d_pwr;
// motor[motorC] = motorpower - d_pwr;
//ERROR CHECKING OR SHUTDOWN
Console.WriteLine(iter + "\t" + u + "\t" + pid + "\t" + th + "\t" + motorpower
+ "\t" + d_pwr + "\t" + motor[motorB] + "\t" + motor[motorC]);
if (pid.Equals(float.NaN) || Math.Abs(th) > 60 || Math.Abs(motorpower) > 2000)
{
// StopAllTasks();
Console.WriteLine("error");
ev3.stopAll();
break;
}
ev3.onMotorA(motor [motorB]);
ev3.onMotorD(motor [motorC]);
// ev3.onMotorA (motor [motorB]/100);
// ev3.onMotorD (motor [motorC]/100);
// //WAIT THEN REPEAT
// while(time1[T4] < dt*1000){
// wait1Msec(1);
// }
// ClearTimer(T4);
Thread.Sleep((int)(dt * 1000));
}
}
