Revision 1:ce487c9929dd, committed 2018-10-22

Comitter:

MaikOvermars

Date:

Mon Oct 22 11:10:41 2018 +0000

Parent:

0:ef81b9f14f58

Commit message:

Updated some code and started implementing kinematics.

Changed in this revision

diff -r ef81b9f14f58 -r ce487c9929dd help_functions/PID_controller.h
--- a/help_functions/PID_controller.h	Fri Oct 19 07:50:39 2018 +0000
+++ b/help_functions/PID_controller.h	Mon Oct 22 11:10:41 2018 +0000
@@ -1,8 +1,13 @@
 #include "mbed.h"
 
+double Kp = 7.5;
+double Ki = 1.02;
+double Kd = 10;
+double samplingfreq = 1000;
+
 double PID_controller(double error)
 {
-    Kp = potmeter2.read()*3;
+    //Kp = potmeter2.read()*3;
     
     double u_k = Kp * error;
     

diff -r ef81b9f14f58 -r ce487c9929dd help_functions/kinematics.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/help_functions/kinematics.h	Mon Oct 22 11:10:41 2018 +0000
@@ -0,0 +1,24 @@
+#include "mbed.h"
+
+double L1 = 0.5;
+double L2 = 0.7;
+double x01 = 0.0;
+double y01 = 0.2;
+double q1,q2,x,y;
+
+float forwardkinematics_function(double motor_angle) {
+    // input are joint angles, output are x and y position of end effector
+    q1 = motor_angle;
+    q2 = motor_angle;
+    
+    x = x01 - L1 * sin(q1) + L2 * cos(q1 + q2);
+    y = y01 + L1 * cos(q1) + L2 * sin(q1 + q2);
+    return x;    
+}
+
+float inversekinematics_function(double reference) {
+    // pseudo inverse jacobian to get joint speeds
+    // input are desired vx and vy of end effector, output joint angle speeds
+    
+    return 5.5;    
+}
\ No newline at end of file

diff -r ef81b9f14f58 -r ce487c9929dd help_functions/processing_chain_emg.h

diff -r ef81b9f14f58 -r ce487c9929dd help_functions/processing_chain_emg1.h

diff -r ef81b9f14f58 -r ce487c9929dd help_functions/processing_chain_emg2.h

diff -r ef81b9f14f58 -r ce487c9929dd help_libraries/HIDScope.lib
--- a/help_libraries/HIDScope.lib	Fri Oct 19 07:50:39 2018 +0000
+++ b/help_libraries/HIDScope.lib	Mon Oct 22 11:10:41 2018 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/tomlankhorst/code/HIDScope/#eade4ec5282b
+http://mbed.org/users/tomlankhorst/code/HIDScope/#d23c6edecc49

diff -r ef81b9f14f58 -r ce487c9929dd help_libraries/MODSERIAL.lib
--- a/help_libraries/MODSERIAL.lib	Fri Oct 19 07:50:39 2018 +0000
+++ b/help_libraries/MODSERIAL.lib	Mon Oct 22 11:10:41 2018 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/Sissors/code/MODSERIAL/#31db07ebcb68
+http://mbed.org/users/Sissors/code/MODSERIAL/#da0788f0bd77

diff -r ef81b9f14f58 -r ce487c9929dd main.cpp
--- a/main.cpp	Fri Oct 19 07:50:39 2018 +0000
+++ b/main.cpp	Mon Oct 22 11:10:41 2018 +0000
@@ -4,37 +4,48 @@
 #include "HIDScope.h"
 #include "BiQuad.h"
 #include "PID_controller.h"
-#include "Servo.h"
+#include "kinematics.h"
 
 //Define objects
+MODSERIAL pc(USBTX, USBRX);
+HIDScope    scope(2);
+
 AnalogIn    emg0( A0 );
 AnalogIn    emg1( A1 );
-HIDScope    scope(2);
+
 PwmOut      motor1_pwm(D5);
+DigitalOut  motor1_dir(D4);
 PwmOut      motor2_pwm(D7);
 DigitalOut  motor2_dir(D6);
-DigitalOut  directionpin(D4);
+
 AnalogIn    potmeter1(A2);
 AnalogIn    potmeter2(A3);
 DigitalIn   button(D0);
+
 Ticker Sample;
-Timer t;
+Timer state_timer;
 
-enum States {failure, wait, calib_enc, calib_emg, operational, demo}; //All possible robot states
+enum States {failure, waiting, calib_enc, calib_emg, operational, demo}; //All possible robot states
 
 //Global variables/objects
 States current_state;
 Ticker loop_ticker; //The Ticker object that will ensure perfect timing of our looping code
-float u, emg_signal_raw_1, processed_emg_1, robot_end_point, motor1pwm, motor1dir, motor2pwm, motor2dir, motor_angle, motor_counts; //will be set by the motor_controller function
+float e, u1, u2, emg_signal_raw_0, processed_emg_0, emg_signal_raw_1, processed_emg_1, robot_end_point, reference_end_point, motor_angle, motor_counts, q_ref; //will be set by the motor_controller function
+int counts_per_rotation = 32;
+bool state_changed = false;
+
+float processing_chain_emg(int num) {
+    return 6.0;
+}
 
 void measure_all() 
 {
     motor_angle = motor_counts*2.0f*3.1415926535f/counts_per_rotation; //do this here, and not in the encoder interrupt, to reduce computational load
-    robot_end_point = forwardkinematics_function();  //motor_angle is global, this function ne
-    emg_signal_raw_1 = emg1.read(); //sample analog voltages (all sampling theory applies, you might get aliasing etc.)
-    emg_signal_raw_2 = emg2.read();
-    processed_emg_1 = processing_chain_emg_1();  // some function ‘float my_emg_processing_chain()’ that returns a float. The raw emg is global
-    processed_emg_2 = processing_chain_emg_2();
+    robot_end_point = forwardkinematics_function(motor_angle);  //motor_angle is global, this function ne
+    emg_signal_raw_0 = emg0.read(); //sample analog voltages (all sampling theory applies, you might get aliasing etc.)
+    emg_signal_raw_1 = emg1.read();
+    processed_emg_0 = processing_chain_emg(0);  // some function ‘float my_emg_processing_chain()’ that returns a float. The raw emg is global
+    processed_emg_1 = processing_chain_emg(1);
 }
 
 void output_all() {
@@ -42,14 +53,15 @@
     motor1_dir = u1 > 0.5f;
     motor2_pwm = fabs(u2);
     motor2_dir = u2 > 0.5f;
+    static int output_counter = 0;
     output_counter++;
-    if (output_counter == 100) {serial.printf(“Something something... %f”,u); output_counter = 0;}  //Print to screen at 10 Hz with MODSERIAL
+    if (output_counter == 100) {pc.printf("Something something... %f",u1); output_counter = 0;}  //Print to screen at 10 Hz with MODSERIAL
 }
 
 void state_machine() {
     switch(current_state) { //States can be: failure, wait, calib_enc, calib_emg, operational, demo,
-        case wait:  //Nothing useful here, maybe a blinking LED for fun and communication to the user
-            if (calib_button.read()==true) 
+        case waiting:  //Nothing useful here, maybe a blinking LED for fun and communication to the user
+            if (button.read()==true) 
             {
                 current_state = calib_enc; //the NEXT loop we will be in calib_enc state
             }
@@ -58,12 +70,13 @@
         case calib_enc:
             if (state_changed==true) 
             {
-                state_time.reset();
+                state_timer.reset();
                 state_timer.start();
                 state_changed = false;
             }
-            u = 0.6f; //a low PWM value to move the motors slowly (0.0 to 0.45 don’t do much due to friction)
-            if (fabs(motor1.velocity()) < 0.1f && state_timer.read() > 5.0f) {
+            u1 = 0.6f; //a low PWM value to move the motors slowly (0.0 to 0.45 don’t do much due to friction)
+            // fabs(motor1.velocity()) < 0.1f && 
+            if (state_timer.read() > 5.0f) {
                 current_state = calib_emg; //the NEXT loop we will be in calib_emg state
                 state_changed = true;
             }
@@ -71,29 +84,34 @@
             
         case calib_emg:     //calibrate emg-signals
             
+            break;
         
         case operational:       //interpreting emg-signals to move the end effector
-            if (state_changed==true) { ... }
-            end_effector_reference_position = some_function(processed_emg);
-            if (... some state tr. guard ...) { ... }
+            if (state_changed==true) { int x = 5; }
+            // example
+            reference_end_point = robot_end_point + processed_emg_0;
+            if (button.read() == true) { current_state = demo; }
             
             break;
             
         case demo: //moving according to a specified trajectory
             
+            if (button.read() == true) { current_state = demo; }
+            
             break;
         
         case failure: //no way to get out
-            u = 0.0f;
+            u1 = 0.0f;
             break;
+    }
 }
 
 void motor_controller() 
 {
     if (current_state >= operational) { // we can (ab)use the fact that an enum is actually an integer, so math/logic rules still apply
-        q_ref = inverse_kinematics(end_effector_reference_position); //many different states can modify your reference position, so just do the inverse kinematics once, here
-        e = q_ref – q_meas; //tracking error
-        u = PID_controller(e); //feedback controller or with possibly fancy controller additions...;
+        q_ref = inversekinematics_function(reference_end_point); //many different states can modify your reference position, so just do the inverse kinematics once, here
+        e = q_ref - motor_angle; //tracking error (q_ref - q_meas)
+        u1 = PID_controller(e); //feedback controller or with possibly fancy controller additions...;
         } //otherwise we just don’t mess with the value of control variable ‘u’ that is set somewhere in the state-machine.
 }
 
@@ -105,15 +123,15 @@
     output_all();               //Output relevant signals, messages, screen outputs, LEDs etc.
 }
 
+
 int main() 
 {
     pc.baud(115200);
-    pwmpin.period_us(60);
-    current_state = wait; //we start in state ‘wait’ and current_state can be accessed by all functions
-    u = 0.0f; //initial output to motors is 0.
+    motor1_pwm.period_us(60);
+    motor2_pwm.period_us(60);
+    current_state = waiting; //we start in state ‘waiting’ and current_state can be accessed by all functions
+    u1 = 0.0f; //initial output to motors is 0.
     loop_ticker.attach(&loop_function, 0.001f); //Run the function loop_function 1000 times per second
     
-    float counts_per_rotation = 32;
-    
     while (true) { }  //Do nothing here (timing purposes)
 }