Bram S
BIOROBOTICS
Dependencies: BrocketJacobian MotorThrottle Olimex_wrapper RemoteIR Servo mbed
Revision 0:df93928b266c, committed 2017-10-30
- Comitter:
- BramS23
- Date:
- Mon Oct 30 13:53:06 2017 +0000
- Commit message:
- Biorobotics
Changed in this revision
diff -r 000000000000 -r df93928b266c BrocketJacobian.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/BrocketJacobian.lib Mon Oct 30 13:53:06 2017 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/BramS23/code/BrocketJacobian/#92c31f1d4274
diff -r 000000000000 -r df93928b266c MotorThrottle.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MotorThrottle.lib Mon Oct 30 13:53:06 2017 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/BramS23/code/MotorThrottle/#f808e6189b2b
diff -r 000000000000 -r df93928b266c Olimex_wrapper.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Olimex_wrapper.lib Mon Oct 30 13:53:06 2017 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/Thijs12va/code/Olimex_wrapper/#02d31a0caac1
diff -r 000000000000 -r df93928b266c RemoteIR.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/RemoteIR.lib Mon Oct 30 13:53:06 2017 +0000 @@ -0,0 +1,1 @@ +http://os.mbed.com/users/shintamainjp/code/RemoteIR/#268cc2ab63bd
diff -r 000000000000 -r df93928b266c Servo.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Servo.lib Mon Oct 30 13:53:06 2017 +0000 @@ -0,0 +1,1 @@ +http://os.mbed.com/users/simon/code/Servo/#36b69a7ced07
diff -r 000000000000 -r df93928b266c main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Mon Oct 30 13:53:06 2017 +0000
@@ -0,0 +1,291 @@
+#include "mbed.h"
+#include "QEI.h"
+#include "PID.h"
+#include "Motor.h"
+#include "BrocketJacobian.h"
+#include "ReceiverIR.h"
+#include "Servo.h"
+#include "emg.h"
+
+// Declare constants etc
+float Interval=0.02f;
+float pi=3.14159265359;
+
+// Declare Analogin in for Potmeter, Can be used for references.
+AnalogIn PotMeter1(A0);
+AnalogIn PotMeter2(A1);
+
+// Declare IR receiver
+ReceiverIR ir_rx(D2);
+
+// Declare motor objects that will control the motor
+Motor Motor1(D5,D4,D12,D13,D9,Interval);
+Motor Motor2(D6,D7,D10,D11,D8,Interval);
+
+// Declare EMG shields and variables
+emg_shield emg1(A0,500);
+emg_shield emg2(A1,500);
+emg_shield emg3(A2,500);
+bool EMG_Direction = 0;
+InterruptIn DirectionButton(D6); // Switch 2
+
+// Declare tickers
+Ticker ControlTicker;
+Ticker GetRefTicker;
+
+// Delare the GYset and GXset, which are the positions derived from integrating
+// after the applying the jacobian inverse
+float GXset = 40.5; //from EMG in cm
+float GYset = 11; //from EMG in cm
+
+// Constant robot parameters
+const float L1 = 27.5f; //length arm 1 in cm
+const float L2 = 32.0f; //length arm 2 in cm
+
+// Motor angles in radialen
+float q1set = 0.25f*pi;
+float q2set = -0.5f*pi;
+
+// Declare stuff for the IR receiver
+RemoteIR::Format format;
+uint8_t buf[4];
+
+// Declare serial object for setting the baudrate
+RawSerial pc(USBTX,USBRX);
+
+void DirectionButtonPressed(){
+ EMG_Direction=!EMG_Direction;
+}
+
+// Looping function using the Jacobian transposed
+void LoopFunctionJacTransposed()
+{
+ float q1 = Motor1.GetPos();
+ float q2 = Motor2.GetPos();
+
+ //start values
+ float tau1 = 0.0f; //previous values are irrelevant
+ float tau2 = 0.0f; //previous values are irrelevant
+
+ float Xcurr = 0.0f; //new value is calculated, old replaced
+ float Ycurr = 0.0f; //new value is calculated, old replaced
+
+ float b = 200.0f; //damping
+ float k = 0.05f; //stiffness of the spring pulling on the end effector
+
+ // Call function to calculate Xcurr and Ycurr from q1 and q2
+ Brocket(q1, q2, Xcurr, Ycurr);
+
+ // Compute spring forces
+ float Fx = k*(GXset-Xcurr);
+ float Fy = k*(GYset-Ycurr);
+
+ // Call function to calculate tau1 and tau2 from X,Ycurr and X,Yset
+ InverseJacobian(q1, q2, Fx, Fy, tau1, tau2);
+
+ // torque to joint velocity
+ float omg1 = tau1/b;
+ float omg2 = tau2/b;
+
+ // joint velocity to angles q1 and q2, where you define new q1 and q2 based on previous q1 and q2
+ q1set = q1set + omg1*Interval;
+ q2set = q2set + omg2*Interval;
+
+ // Call the function that controlls the motors
+ Motor1.GotoPos(q1set);
+ Motor2.GotoPos(q2set);
+}
+
+
+void LoopJacInverse(){
+ // Get Motor Positions
+ float q1 = Motor1.GetPos();
+ float q2 = Motor2.GetPos();
+
+ // Define velocities for control
+ float q1_dot=0.0f;
+ float q2_dot=0.0f;
+
+ // Get the velocities from EMG
+ float vx=0.0f;
+ float vy=0.0f;
+
+ // Apply Jacobian Inverse
+ InverseJacobian(q1,q2,vx,vy,q1_dot,q2_dot);
+
+ // Integrate q1dot and q2dot to obtain positions
+ q1set += q1_dot*Interval;
+ q2set += q2_dot*Interval;
+
+ // Call the motor control functions
+ Motor1.GotoPos(q1set);
+ Motor2.GotoPos(q2set);
+}
+
+
+// Start homing the motors
+void HomingLoop(){
+ // with param:(Direction , PWM , Home pos in radians)
+ Motor1.Homing(1,0.2f,0.29f);
+ Motor2.Homing(1,0.1f,(3.3715f-2.0f*pi));
+}
+
+// Function for picking up a checkers playthingy
+void PickUp(){
+
+}
+
+// Function for dropping a checkers playthingy
+void LayDown(){
+
+}
+
+// Forward declarate remote controller function
+void RemoteController();
+
+
+// Give Reference Position
+void DeterminePosRef(){
+ GXset=40*PotMeter1.read(); // Reference in Rad
+ GYset=40*PotMeter2.read(); // Reference in Rad
+}
+
+int main() {
+ pc.baud(115200);
+ pc.printf("Program BIOROBOTICS startup\r\n");
+
+ // Define Controller Values
+ Motor1.SetInputLimits(-2.0f*pi,2.0f*pi);
+ Motor1.SetInputLimits(-2.0f*pi,2.0f*pi);
+
+ Motor2.SetOutputLimits(-0.15f,0.15f);
+ Motor2.SetOutputLimits(-0.5f,0.5f);
+
+ Motor1.SetPID(100.0f,0.0f,0.001f);
+ Motor2.SetPID(100.0f,0.0f,0.001f);
+
+ Motor1.SetGearRatio(3.0f);
+ Motor2.SetGearRatio(1.8f);
+
+ // Start homing function
+ pc.printf("Homing \r\n");
+ HomingLoop();
+
+ // Start Tickers
+ pc.printf("Starting Tickers \r\n");
+ ControlTicker.attach(&LoopFunctionJacTransposed,Interval);
+ GetRefTicker.attach(&DeterminePosRef,0.5f);
+ DirectionButton.rise(&DirectionButtonPressed);
+
+ // Check wheater a remote command has been send
+ while(1)
+ {
+ if (ir_rx.getState() == ReceiverIR::Received)
+ {
+ pc.printf("received");
+
+
+ RemoteController();
+ wait(0.1);
+ }
+ }
+
+
+ while(1){}
+
+
+}
+
+
+
+void RemoteController(){
+
+ int bitcount;
+
+
+ bitcount = ir_rx.getData(&format, buf, sizeof(buf) * 8);
+ int state = buf[2];
+ switch(state)
+ {
+ case 22: //1
+ pc.printf("1\n\r");
+ break;
+ case 25: //1
+ pc.printf("2\n\r");
+ break;
+ case 13: //1
+ pc.printf("3\n\r");
+ break;
+ case 12: //1
+ pc.printf("4\n\r");
+ break;
+ case 24: //1
+ pc.printf("5\n\r");
+ ControlTicker.detach();
+ Motor1.Stop();
+ Motor2.Stop();
+ PickUp();
+ //ControlTicker.attach(&ControlLoop, Interval);
+ break;
+ case 94: //1
+ pc.printf("6\n\r");
+ break;
+ case 8: //1
+ pc.printf("7\n\r");
+ break;
+ case 28: //1
+ pc.printf("8\n\r");
+ ControlTicker.detach();
+ Motor1.Stop();
+ Motor2.Stop();
+ LayDown();
+ //ControlTicker.attach(&ControlLoop, Interval);
+ break;
+ case 90: //1
+ pc.printf("9\n\r");
+ break;
+ case 70: //1
+ pc.printf("Boven\n\r");
+ //PosRef2=PosRef2-0.1f;
+ /*
+ GYset = GYset + 1;
+ */
+ //pc.printf("%f\n\r", PosRef2);
+ break;
+ case 21: //1
+ pc.printf("Onder\n\r");
+ //PosRef2=PosRef2+0.1f;
+ /*
+ GYset = GYset - 1;
+ */
+ //pc.printf("%f\n\r", PosRef2);
+ break;
+ case 68: //1
+ pc.printf("Links\n\r");
+ //PosRef1=PosRef1+0.1f;
+ /*
+ GXset = GXset + 1;
+ */
+ //pc.printf("%f\n\r", PosRef1);
+ break;
+ case 67: //1
+ pc.printf("Rechts\n\r");
+ //PosRef1=PosRef1-0.1f;
+ /*
+ GXset = GXset - 1;
+ */
+ //pc.printf("%f\n\r", PosRef1);
+ break;
+ case 64: //1
+ pc.printf("OK\n\r");
+ //ControlTicker.detach();
+ //MotorThrottle1=0.0f;
+ //MotorThrottle2=0.0f;
+ //HomingLoop();
+ //ControlTicker.attach(&ControlLoop, Interval);
+
+ break;
+ default:
+ break;
+ }
+}
\ No newline at end of file
diff -r 000000000000 -r df93928b266c mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Mon Oct 30 13:53:06 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/fb8e0ae1cceb \ No newline at end of file