mevlid yildirim
Minor BioRobotics BMT Hierbij publish ik mijn code public ter inspiratie voor komende jaarlagen. Het gaat om een serial robot met twee links en een haak als end-effector. Veel plezier ermee!
Dependencies: mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 25:d94275968963
- Parent:
- 24:f8482c47a385
--- a/main.cpp Thu Oct 31 12:45:16 2019 +0000
+++ b/main.cpp Thu Oct 31 15:00:30 2019 +0000
@@ -53,13 +53,13 @@
double x_max= 30.5;
double x_min= 8.5;
double x_home=8.5;
- double x_vergroting=0.3;
+ double x_vergroting=0.01;
double y = 0; // 7.5 < y < 27
double y_max=27;
double y_min=7.5;
double y_home=7.5;
- double y_vergroting=0.03;
+ double y_vergroting=0.01;
double a;
double b;
double c;
@@ -71,20 +71,20 @@
bool emg3_active;
// 3.3 EMG Variables **********************************************************
-static BiQuad LowPassFilter_motor1( 3.46041e-04, 6.92083e-04, 3.46041e-04, -1.94670e+00, 9.48082e-01 );
-static BiQuad LowPassFilter_motor2( 3.46041e-04, 6.92083e-04, 3.46041e-04, -1.94670e+00, 9.48082e-01 );
+static BiQuad LowPassFilter_motor1( 4.12535e-02, 8.25071e-02, 4.12535e-02, -1.34897e+00, 5.13982e-01 );
+static BiQuad LowPassFilter_motor2( 4.12535e-02, 8.25071e-02, 4.12535e-02, -1.34897e+00, 5.13982e-01 );
-static BiQuad highfilter0(3.11539e-01, -6.23078e-01, 3.11539e-01, -7.36238e-02, 1.72531e-01 );
-static BiQuad LowPassFilter0( 3.46041e-04, 6.92083e-04, 3.46041e-04, -1.94670e+00, 9.48082e-01);
+static BiQuad highfilter0(9.56543e-01, -1.91309e+00, 9.56543e-01, -1.91120e+00, 9.14976e-01);
+static BiQuad LowPassFilter0( 4.12535e-02, 8.25071e-02, 4.12535e-02, -1.34897e+00, 5.13982e-01 );
-static BiQuad highfilter1(3.11539e-01, -6.23078e-01, 3.11539e-01, -7.36238e-02, 1.72531e-01 );
-static BiQuad LowPassFilter1( 3.46041e-04, 6.92083e-04, 3.46041e-04, -1.94670e+00, 9.48082e-01 );
+static BiQuad highfilter1(9.56543e-01, -1.91309e+00, 9.56543e-01, -1.91120e+00, 9.14976e-01);
+static BiQuad LowPassFilter1( 4.12535e-02, 8.25071e-02, 4.12535e-02, -1.34897e+00, 5.13982e-01 );
-static BiQuad highfilter2(3.11539e-01, -6.23078e-01, 3.11539e-01, -7.36238e-02, 1.72531e-01 );
-static BiQuad LowPassFilter2( 3.46041e-04, 6.92083e-04, 3.46041e-04, -1.94670e+00, 9.48082e-01 );
+static BiQuad highfilter2(9.56543e-01, -1.91309e+00, 9.56543e-01, -1.91120e+00, 9.14976e-01);
+static BiQuad LowPassFilter2( 4.12535e-02, 8.25071e-02, 4.12535e-02, -1.34897e+00, 5.13982e-01 );
-static BiQuad highfilter3(3.11539e-01, -6.23078e-01, 3.11539e-01, -7.36238e-02, 1.72531e-01 );
-static BiQuad LowPassFilter3( 3.46041e-04, 6.92083e-04, 3.46041e-04, -1.94670e+00, 9.48082e-01 );
+static BiQuad highfilter3(9.56543e-01, -1.91309e+00, 9.56543e-01, -1.91120e+00, 9.14976e-01);
+static BiQuad LowPassFilter3( 4.12535e-02, 8.25071e-02, 4.12535e-02, -1.34897e+00, 5.13982e-01 );
double emg0_raw_signal ; double emg1_raw_signal ; double emg2_raw_signal ; double emg3_raw_signal ;
double high_emg0_signal ; double high_emg1_signal ; double high_emg2_signal ; double high_emg3_signal ;
@@ -99,11 +99,11 @@
DigitalOut led_red(LED_RED); // Defines the red led on the K64 board (0=on, 1 = off)
DigitalOut led_green(LED_GREEN); // Defines the green led on the K64 board (0=on, 1 = off)
DigitalOut led_blue(LED_BLUE); // Defines the blue led on the K64 board (0=on, 1 = off)
-// FastPWM led1(D8); //CODE DOES NOT WORK WITH D8 PIN DEFINED //Defines Led1 on the BioRobotics Shield
- FastPWM led2(D9); //Defines Led2 on the BioRobotics Shield
+ FastPWM led1(D9); //CODE DOES NOT WORK WITH D8 PIN DEFINED //Defines Led1 on the BioRobotics Shield
+ FastPWM led2(A5); //Defines Led2 on the BioRobotics Shield
//3.4b Potmeters and buttons
- AnalogIn pot1_links(A5); //Defines potmeter1 on the BioRobotics Shield
+ // AnalogIn pot1_links(A5); //BUITEN GEBRUIK Defines potmeter1 on the BioRobotics Shield
AnalogIn pot2_rechts(A4); //Defines potmeter2 on the BioRobotics Shield
DigitalIn button1(D2); //Defines button1 on the BioRobotics Shield
DigitalIn button2(D3); //Defines button2 on the BioRobotics Shield
@@ -133,6 +133,7 @@
int emg_tijd=0;
bool calibration_done=false;
int homing_tijd=0;
+ int demo_tijd=0;
// 3.5 Motor 1 variables ***************************************************************
//3.5a PID-controller motor 1
@@ -186,8 +187,11 @@
void HIDScope() //voor HIDscope
{
scope.set(0, emg0_signal);
- scope.set(1, emg0_max);
- scope.set(2, x);
+ scope.set(1, emg1_signal);
+ scope.set(2, emg2_signal);
+ scope.set(3, emg3_signal);
+
+
// scope.set(4, Ui_motor1);
// scope.set(5, Uk_motor1);
@@ -427,22 +431,30 @@
double RKI()
{
if (emg0_active==true)
- { if (x>x_max) {x=x_max;}
- else { x=x+ x_vergroting ; led_blue.write(0); }
+ {
+ led1=1;
+ if (x>x_max) {x=x_max;}
+ else { x=x+ x_vergroting ; }
}
if (emg1_active==true)
- { if (y>y_max) {y=y_max;}
- else {y=y + y_vergroting; }
+ {
+ led2=1;
+ if (y>y_max) {y=y_max;}
+ else {y=y + y_vergroting ; }
}
if (emg2_active==true)
- { if (x<x_min) {x=x_min;}
+ {
+ led1=0;
+ if (x<x_min) {x=x_min;}
else {x = x - x_vergroting; }
}
if (emg3_active==true)
- { if (y<y_min) {y=y_min;}
+ {
+ led2=1;
+ if (y<y_min) {y=y_min;}
else {y=y - y_vergroting;}
}
@@ -453,9 +465,10 @@
void motor1_controller(void)
{
error1_motor1 = (theta_motor1 - positie_motor1);
+
if (motor1_calibrated==true&&motor2_calibrated==true)
{
- motor1_pwm();
+ motor1_pwm();
}
}
@@ -478,18 +491,20 @@
{
case MotorCalibration:
// pc.printf("\r\n State: MotorCalibration");
- led_blue.write(1);
+ led_blue.write(0);
led_red.write(1);
- led_green.write(0); //Green Led on when in this state
+ led_green.write(1); //Green Led on when in this state
if (motor1_calibrated==true&&motor2_calibrated==true)
{
pc.printf("\r\n Motor Calibration is done!");
+ x=28;
+ y=11;
encoder_motor1.reset();
encoder_motor2.reset();
- State=StartWait;
+ State=Demo; //StartWait;
}
else {;} //pc.printf("\r\n Motor Calibration is not done!");}
@@ -499,10 +514,10 @@
// pc.printf("\r\n State: StartWait Button 1 = operation, Button 2 = Demo");
led_blue.write(0);
led_red.write(1);
- led_green.write(1);
+ led_green.write(0);
emg_tijd =0;
- calibration_done=true;
+ calibration_done=false;
State=EMGCalibration;
break;
@@ -514,10 +529,10 @@
emg_tijd++;
- if (0<=emg_tijd&&emg_tijd<=5000)
- { led_green.write(0); }
- else if (5000<emg_tijd&&emg_tijd<8000)
- { led_green.write(1);
+ if (0<=emg_tijd&&emg_tijd<=3000)
+ { led_green.write(1); }
+ else if (3000<emg_tijd&&emg_tijd<6000)
+ { led_green.write(0);
if (emg0_signal > emg0_max)
{
emg0_max=emg0_signal;
@@ -538,8 +553,9 @@
emg3_max=emg3_signal;
}
}
- else if (8000<emg_tijd)
- {led_green.write(0);
+ else if (6000<emg_tijd)
+ {led_green.write(1);
+
calibration_done=true; // later verplaatsen
if(button1==0) {State=StartWait;}
if(button2==0) {State=Homing;}
@@ -558,8 +574,9 @@
y= y_home;
if (homing_tijd>1000) //Voorkomt dat het automatisch naar Demo springt, omdat je bij de vorige nog knop 2 hebt ingedrukt
{
- if(button1==0) {State=Operating;}
- if(button2==0) {State=Demo;}
+ if(button1==0) {State=Demo;}
+ if(button2==0) {State=Operating;}
+
}
break;
@@ -573,10 +590,23 @@
break;
case Demo:
- pc.printf("\r\n State: Demo");
+ // pc.printf("\r\n State: Demo");
led_blue.write(1);
- led_red.write(1);
- led_green.write(0);
+ led_red.write(0);
+ led_green.write(1);
+ demo_tijd++;
+
+ if (button1==0)
+ { led1=1;
+ if (x>x_max) {x=x_max;}
+ else { x=x+ x_vergroting ;}
+ }
+ if (button2==0)
+ { led1=0;
+ if (x<x_min) {x=x_min;}
+ else { x=x - x_vergroting ;}
+ }
+
break;
@@ -668,14 +698,14 @@
led_green.write(1);
led_red.write(1);
led_blue.write(1);
- State = EMGCalibration ; // veranderen naar MotorCalibration;
+ State = MotorCalibration ; // veranderen naar MotorCalibration;
ticker_hidscope.attach(&HIDScope, 0.001); //Ticker for Hidscope, different frequency compared to motors
ticker_mainloop.attach(&main_loop,0.001); // change back to 0.001f //Run the function main_loop 1000 times per second
- // motor_calibration();
+ motor_calibration();
// 6.2 While loop in main function**********************************************
- while (true) { pc.printf("\r\n y = %f, theta_motor2 = %f, error1_motor2 = %f", y, theta_motor2, error1_motor2);} //Is not used but has to remain in the code!!
+ while (true) { wait(0.5); pc.printf("\r\n x = %f, y = %f, \n theta_motor1 = %f, theta_motor2 = %f, error1_motor1 = %f", x,y, theta_motor1, theta_motor2, error1_motor1);} //Is not used but has to remain in the code!!
} //Ending of Main() Function