Fernon Eijkhoudt
EMG driven robot which shoots elastic bands
Fork of
RoboBirdV1
by 
Fernon Eijkhoudt
Diff: main.cpp
- Revision:
- 40:cac08f589732
- Parent:
- 39:e5bf4b1293fa
--- a/main.cpp Thu Oct 29 09:45:12 2015 +0000
+++ b/main.cpp Thu Oct 29 15:15:45 2015 +0000
@@ -4,18 +4,18 @@
// Motor 1 & 2
-DigitalOut Direction(D4); //1 = CCW - 0 = CW, moet nog omgezet worden naar up en down
-PwmOut PowerMotor(D5); //van 0 tot 1
-QEI Encoder(D10,D11,NC,32,QEI::X2_ENCODING); //Encoder
+DigitalOut Direction(D4);
+PwmOut PowerMotor(D5);
+QEI Encoder(D10,D11,NC,32,QEI::X2_ENCODING);
DigitalOut Direction2(D7);
PwmOut PowerMotor2(D6);
QEI Encoder2(D12,D13,NC,32,QEI::X2_ENCODING);
PwmOut PowerServo(D3);
// Buttons & EMG
-AnalogIn Button(A3); // Calibration Button
+AnalogIn Button(A3); // Calibration button
AnalogIn Button2(A4); // Fire button
-DigitalIn Button3(D9);
+DigitalIn Button3(D9); // Home button
AnalogIn EMG(A0);
AnalogIn EMG2(A1);
@@ -32,37 +32,31 @@
-// Waardes
+// Values
const double twopi = 6.2831853071795;
-const double Fs=100;
+const double Fs=100; // Sample Frequency
int Fired = 0;
-// EMG
-double T1 = 0;
-double T2 = 0;
-double T3 = 0;
-double T4 = 0;
-
-// Motor 1 (Translatie)
-const double n1 = 3.3; // Aantal rondjes dat ons apparaat maximaal mag draaien (omhoog)
+// Motor 1 (Translation)
+const double n1 = 3.3; // The amount of the rotations that is allowed
int Pulses;
-double Rotatie = 0; //aantal graden dat de motor is gedraaid
-double Goal; //initele waarde goal waar de motor naar toe moet, dit wordt gedaan
-double Error = 0;
+double Rotatie = 0; //Rotation in radial
+double Goal; // Setpoint for Motor
+double Error = 0; // Setting initial values
double Errord = 0;
double Errori = 0;
double Errorp = 0;
-const double Kp = 0.2; //Moet berekend worden aan de hand van Control concept slides
+const double Kp = 0.2; // PID controll parameters
const double Kd = 10;
const double Ki = 0.2;
double v = 0;
-double upperlimit; //max aantal rotaties omhoog
-const double downlimit = 0.8;
-const double margin = 0.8;
-bool OutRange = false;
+double upperlimit; //Maximum rounds up
+const double downlimit = 0.8; //Lowest limit of the robot
+const double margin = 0.8; //Margin for safe range
+bool OutRange = false; //In Range or not
-// Motor 2 (Rotatie)
-const double n2 = 0.4; // Aantal rondjes dat dat ons apparaat maximaal mag draaien (rotatie)
+// Motor 2 (Rotation)
+const double n2 = 0.334;
int Pulses2;
double Rotatie2 = 0;
double Goal2;
@@ -74,22 +68,26 @@
const double Kd2 = 10;
const double Ki2 = 0.2;
double v2 = 0;
-double turnlimit = 0.4; // max aantal rotaties voor roteren hele robot
-// Margin 2 is in ons geval 0
+double turnlimit = 0.4;
+// Margin 2 = 0
bool OutRange2 = false;
-// Activatie tussen het schietgedeelte en terugkeergedeelte
+// Switch between firing mode (Excecute) and return to Home mode
bool Excecute = false;
bool Home = false;
// Filter
-double Fs2 = 500; // in Hz
-const double TijdCal = 5;
-double Max = 0;
-double Max2 = 0;
-bool Cali = false;
+double Fs2 = 500; // Frequency in Hz
+const double TijdCal = 5; // Time for calibration
+double Max = 0; // Max value for EMG
+double Max2 = 0; // Max value for EMG2
+bool Cali = false; // Switch for Calibration
+double T1 = 0; // Thresholds for EMG
+double T2 = 0;
+double T3 = 0;
+double T4 = 0;
-double u1;
+double u1; //All variables for filtering
double y1;
double y2;
double y3;
@@ -149,7 +147,7 @@
-// Voids voor berekeningen in het hoofdprogramma
+// Voids for calculations in the main programme
double biquad(double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2)
{
@@ -207,27 +205,28 @@
void MotorSet()
{
+ // Calculation for position of motor
Pulses = Encoder.getPulses();
Rotatie = (Pulses*twopi)/4200;
Pulses2 = Encoder2.getPulses();
Rotatie2 = (Pulses2*twopi)/4200;
- // Eerst motor 1 (translatie)
- if (OutRange) {
- Error = Goal-Rotatie; // De error die het motortje maakt ten opzichte van je Goal
+ // Motor 1 (translation)
+ if (OutRange) { //If the Robot is OutRange or is going Home
+ Error = Goal-Rotatie;
Errord = (Error-Errorp)/Fs;
Errorp = Error;
- if (fabs(Error) <= 0.5) {
+ if (fabs(Error) <= 0.5) { // Only activate the I part of PID when the Error is small
Errori = Errori + Error*1/Fs;
} else {
Errori = 0;
}
- if (Error>=0) {
+ if (Error>=0) { // Determine the direction
Direction=1;
} else {
Direction=0;
}
- v=Kp*Error + Kd*Errord + Ki*Errori;
- if (Home == true) {
+ v=Kp*Error + Kd*Errord + Ki*Errori; //Calculation of speed
+ if (Home == true) { //Maximum speed when OutRange
if (v >0.15) {
v = 0.15;
}
@@ -237,7 +236,7 @@
// Dan motor 2 (rotatie)
if (OutRange2) {
- Error2 = Goal2-Rotatie2; // De error die het motortje maakt ten opzichte van je Goal
+ Error2 = Goal2-Rotatie2;
Errord2 = (Error2-Errorp2)/Fs;
Errorp2 = Error2;
if (fabs(Error2) <= 0.5) {
@@ -257,10 +256,10 @@
void Calibration ()
{
- if (OutRange == false && OutRange2 == false) {
+ if (OutRange == false && OutRange2 == false) { //Only allow callibration during firing mode and when in Range
if (Button >= 0.8) {
wait (0.2);
- Cali = true;
+ Cali = true; // Calibration part in the EMG void is being activated
TijdEMGCal.start();
Excecute = false;
ledC = 0;
@@ -269,15 +268,14 @@
}
}
if (TijdEMGCal.read() >= TijdCal) {
- Cali = false;
+ Cali = false; // Calibration part in the EMG void is stopped
ledC = 1;
TijdEMGCal.stop();
TijdEMGCal.reset();
- T1 = 0.35*Max;
+ T1 = 0.35*Max; // Determination of thresholds
T2 = 0.5*Max;
T3 = 0.35*Max2;
T4 = 0.5*Max2;
- //pc.printf("T1 = %f\nT2 = %f\nT3 = %f\nT4 = %f\n", T1, T2, T3, T4);
wait (3);
Excecute = true;
}
@@ -285,31 +283,31 @@
int main()
{
- upperlimit = n1*twopi;
+ upperlimit = n1*twopi; // Calculation for limits of the robot
turnlimit = n2*twopi;
ledF = 1;
ledC = 1;
PowerMotor.write(0);
PowerMotor2.write(0);
- MotorWrite.attach(MotorSet,1/Fs);
+ MotorWrite.attach(MotorSet,1/Fs); // Activate the tickers
biquadTicker.attach(myController,1/Fs2);
PowerServo.period_ms(20);
while (true) {
- Calibration();
+ Calibration(); // Makes it able to calibrate during firing mode
while (Excecute) {
- // Eerst wordt motor 1 aangestuurd
- if (Rotatie >= upperlimit) { //Als hij buiten bereik is
+ // Motor 1
+ if (Rotatie >= upperlimit) { // If OutRange
Goal = upperlimit - margin-0.2;
OutRange = true;
}
- if (Rotatie <= downlimit) { //Als hij buiten bereik is
+ if (Rotatie <= downlimit) { // If OutRange
Goal = 0 + margin +0.2;
OutRange = true;
}
- if (Rotatie >= margin && Rotatie <= upperlimit - margin) { // Voor als hij voor het eerst weer binnen bereik is
+ if (Rotatie >= margin && Rotatie <= upperlimit - margin) { // If in range / not OutRange
OutRange = false;
}
- if (Rotatie >= downlimit && Rotatie <= upperlimit && OutRange == false) { //EMG aansturing
+ if (Rotatie >= downlimit && Rotatie <= upperlimit && OutRange == false) { //EMG aansturing when in range / not OutRange
if (y8 >= T2 ) {
Direction = 1;
v = 0.13;
@@ -324,41 +322,45 @@
}
}
- // Vanaf hier wordt motor 2 aangestuurd
- if (Rotatie2 >= turnlimit) { //Als hij buiten bereik is
+ // Motor
+ if (Rotatie2 >= turnlimit) { // If OutRange
Goal2 = turnlimit-0.1;
OutRange2 = true;
}
- if (Rotatie2 <= -turnlimit) { //Als hij buiten bereik is
+ if (Rotatie2 <= -turnlimit) { // If OutRange
Goal2 = -turnlimit+0.1;
OutRange2 = true;
}
- if (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit) { // Voor als hij voor het eerst weer binnen bereik is
+ if (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit) { // If in range / not OutRange
OutRange2 = false;
}
- if (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit && OutRange2 == false) { // EMG aansturing
+ if (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit && OutRange2 == false) { //EMG aansturing when in range / not OutRange
if (y18 >= T4 ) {
Direction2 = 1;
- v2 = 0.07;
+ v2 = 0.06;
}
if (y18 > T3 && y18 < T4) {
Direction2 = 0;
- v2 = 0.07;
+ v2 = 0.06;
}
if (y18 <= T3) {
Direction2 = 0;
v2 = 0;
}
}
- if (Button2 >= 0.8) { //Afvuren van de RBG
+ if (Button2 >= 0.8) { // Firing Rubber Band Gun
wait(0.2);
ledF = 0;
PowerServo.write(0.27);
wait (1);
PowerServo.write(0.04);
+ wait (1);
+ PowerServo.write(0.27);
+ wait (1);
+ PowerServo.write(0.04);
ledF = 1;
Fired=Fired+1;
- if (Fired == 4) {
+ if (Fired == 3) {
wait (1);
Home = true;
Excecute = false;
@@ -371,18 +373,18 @@
Calibration();
}
- while (Home) { //Terugkeren naar vaste positie
- OutRange = true; //Hiermee wordt het PID gedeelte van de motor control aangestuurd.
+ while (Home) { // Return to base position
+ OutRange = true; // Activation of PID control part
OutRange2 = true;
Goal = margin;
Goal2 = 0;
- if (fabs(Error)<=0.1 && fabs(Error2)<=0.1) {
+ if (fabs(Error)<=0.1 && fabs(Error2)<=0.1) { // If error is small enough, then firing mode will be enabled again
timer.start();
} else {
timer.stop();
timer.reset();
}
- if (timer.read() >= 3) {
+ if (timer.read() >= 3) { // Firing mode is being enabled again and values are reset.
Errori = 0;
Errord = 0;
Errorp = 0;