Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed Servo
Revision 46:9b60a3c1acab, committed 2018-11-01
- Comitter:
- Spekdon
- Date:
- Thu Nov 01 20:54:51 2018 +0000
- Parent:
- 45:829a3992d689
- Commit message:
- Presentation code
Changed in this revision
| help_functions/PID_controller.h | Show annotated file Show diff for this revision Revisions of this file |
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/help_functions/PID_controller.h Thu Nov 01 19:48:22 2018 +0000 +++ b/help_functions/PID_controller.h Thu Nov 01 20:54:51 2018 +0000 @@ -2,7 +2,7 @@ double Kp = 0.1; double Ki = 0.4; -double Kd = 4; +double Kd = 5; extern double samplingfreq = 1000; //void PID_controller(double error1, double error2, double &u1, double &u2, const double &T)
--- a/main.cpp Thu Nov 01 19:48:22 2018 +0000
+++ b/main.cpp Thu Nov 01 20:54:51 2018 +0000
@@ -17,9 +17,9 @@
AnalogIn emg1( A1 );
AnalogIn emg2( A2 );
AnalogIn emg3( A3 );
-//Servo myservo(A5);
+Servo gripper(A5);
//InterruptIn button1(SW2);
-//InterruptIn button2(SW3);
+DigitalIn buttongripper(D1);
double range;
// motor ouptuts
@@ -49,6 +49,7 @@
// Timeouts and related variables
Timeout make_button_active;
bool button_suppressed = false;
+bool gripperopen = true;
// States
enum States {failure, waiting, calib_emg, calib_enc, operational, demo, homing}; //All possible robot states
@@ -117,14 +118,6 @@
// Functions
-//void open_gripper(){
-// myservo=0;
-//}
-//
-//void close_gripper(){
-// myservo=1;
-//}
-
vector<double> forwardkinematics_function(double& q1, double& q2) {
// input are joint angles, output are x and y position of end effector
@@ -193,12 +186,13 @@
}
void output_all() {
- motor1_pwm = fabs(u1);
+ motor1_pwm = 0.4+0.6*fabs(u1);
+
if (motor1_pwm > 1.0f){
motor1_pwm = 1.0f;
}
motor1_dir = u1 > 0.0f;
- motor2_pwm = fabs(u2);
+ motor2_pwm = 0.4+0.6*fabs(u2);
if (motor2_pwm > 1.0f){
motor2_pwm = 1.0f;
}
@@ -428,7 +422,15 @@
make_button_active.attach(&unsuppressing_button,0.5);
}
-
+
+ if (buttongripper.read() == false && button_suppressed == false ) {
+ button_suppressed = true;
+ if (gripperopen){gripper = 0; gripperopen = false;}
+ else {gripper = 1; gripperopen = true;}
+
+ make_button_active.attach(&unsuppressing_button,0.5);
+ }
+
break;
case demo: //moving according to a specified trajectory
@@ -533,12 +535,14 @@
led_R = 1;
led_B = 1;
led_G = 1;
-// myservo = 1;
-// button1.fall(&open_gripper);
-// button2.fall(&close_gripper);
- while (true) {pc.printf("error1: %f, error2: %f, pwm1: %f, pwm2: %f, qref1: %f, qref2: %f, vx: %f, vy: %f, q1: %f, q2: %f \r\n", e1, e2, u1, u2, qref1, qref2, des_vx, des_vy, x_norm, y_norm);
- testconfig = forwardkinematics_function(qref1,qref2);
+ gripper = 1;
+ int m = 0;
+ while (true) {
+ m++;
+ if (m%1000)
+ { pc.printf("error1: %f, error2: %f, pwm1: %f, pwm2: %f, qref1: %f, qref2: %f, vx: %f, vy: %f, q1: %f, q2: %f, gripper: \r\n", e1, e2, u1, u2, qref1, qref2, des_vx, des_vy, q1, q2);}
+ //testconfig = forwardkinematics_function(qref1,qref2);
//double x = testconfig[0];
//double y = testconfig[1];
//pc.printf("x: %f, y: %f \r\n", x,y);