Bram S / Mbed 2 deprecated BIOROBOTICS_GROUP24

BIOROBOTICS

Dependencies:   BrocketJacobian MotorThrottle Olimex_wrapper RemoteIR Servo mbed

main.cpp@0:df93928b266c, 2017-10-30 (annotated)

Committer:
BramS23
Date:
Mon Oct 30 13:53:06 2017 +0000
Revision:
0:df93928b266c
Biorobotics

Who changed what in which revision?

UserRevisionLine numberNew contents of line
BramS23 0:df93928b266c 1 #include "mbed.h"
BramS23 0:df93928b266c 2 #include "QEI.h"
BramS23 0:df93928b266c 3 #include "PID.h"
BramS23 0:df93928b266c 4 #include "Motor.h"
BramS23 0:df93928b266c 5 #include "BrocketJacobian.h"
BramS23 0:df93928b266c 6 #include "ReceiverIR.h"
BramS23 0:df93928b266c 7 #include "Servo.h"
BramS23 0:df93928b266c 8 #include "emg.h"
BramS23 0:df93928b266c 9
BramS23 0:df93928b266c 10 // Declare constants etc
BramS23 0:df93928b266c 11 float Interval=0.02f;
BramS23 0:df93928b266c 12 float pi=3.14159265359;
BramS23 0:df93928b266c 13
BramS23 0:df93928b266c 14 // Declare Analogin in for Potmeter, Can be used for references.
BramS23 0:df93928b266c 15 AnalogIn PotMeter1(A0);
BramS23 0:df93928b266c 16 AnalogIn PotMeter2(A1);
BramS23 0:df93928b266c 17
BramS23 0:df93928b266c 18 // Declare IR receiver
BramS23 0:df93928b266c 19 ReceiverIR ir_rx(D2);
BramS23 0:df93928b266c 20
BramS23 0:df93928b266c 21 // Declare motor objects that will control the motor
BramS23 0:df93928b266c 22 Motor Motor1(D5,D4,D12,D13,D9,Interval);
BramS23 0:df93928b266c 23 Motor Motor2(D6,D7,D10,D11,D8,Interval);
BramS23 0:df93928b266c 24
BramS23 0:df93928b266c 25 // Declare EMG shields and variables
BramS23 0:df93928b266c 26 emg_shield emg1(A0,500);
BramS23 0:df93928b266c 27 emg_shield emg2(A1,500);
BramS23 0:df93928b266c 28 emg_shield emg3(A2,500);
BramS23 0:df93928b266c 29 bool EMG_Direction = 0;
BramS23 0:df93928b266c 30 InterruptIn DirectionButton(D6); // Switch 2
BramS23 0:df93928b266c 31
BramS23 0:df93928b266c 32 // Declare tickers
BramS23 0:df93928b266c 33 Ticker ControlTicker;
BramS23 0:df93928b266c 34 Ticker GetRefTicker;
BramS23 0:df93928b266c 35
BramS23 0:df93928b266c 36 // Delare the GYset and GXset, which are the positions derived from integrating
BramS23 0:df93928b266c 37 // after the applying the jacobian inverse
BramS23 0:df93928b266c 38 float GXset = 40.5; //from EMG in cm
BramS23 0:df93928b266c 39 float GYset = 11; //from EMG in cm
BramS23 0:df93928b266c 40
BramS23 0:df93928b266c 41 // Constant robot parameters
BramS23 0:df93928b266c 42 const float L1 = 27.5f; //length arm 1 in cm
BramS23 0:df93928b266c 43 const float L2 = 32.0f; //length arm 2 in cm
BramS23 0:df93928b266c 44
BramS23 0:df93928b266c 45 // Motor angles in radialen
BramS23 0:df93928b266c 46 float q1set = 0.25f*pi;
BramS23 0:df93928b266c 47 float q2set = -0.5f*pi;
BramS23 0:df93928b266c 48
BramS23 0:df93928b266c 49 // Declare stuff for the IR receiver
BramS23 0:df93928b266c 50 RemoteIR::Format format;
BramS23 0:df93928b266c 51 uint8_t buf[4];
BramS23 0:df93928b266c 52
BramS23 0:df93928b266c 53 // Declare serial object for setting the baudrate
BramS23 0:df93928b266c 54 RawSerial pc(USBTX,USBRX);
BramS23 0:df93928b266c 55
BramS23 0:df93928b266c 56 void DirectionButtonPressed(){
BramS23 0:df93928b266c 57 EMG_Direction=!EMG_Direction;
BramS23 0:df93928b266c 58 }
BramS23 0:df93928b266c 59
BramS23 0:df93928b266c 60 // Looping function using the Jacobian transposed
BramS23 0:df93928b266c 61 void LoopFunctionJacTransposed()
BramS23 0:df93928b266c 62 {
BramS23 0:df93928b266c 63 float q1 = Motor1.GetPos();
BramS23 0:df93928b266c 64 float q2 = Motor2.GetPos();
BramS23 0:df93928b266c 65
BramS23 0:df93928b266c 66 //start values
BramS23 0:df93928b266c 67 float tau1 = 0.0f; //previous values are irrelevant
BramS23 0:df93928b266c 68 float tau2 = 0.0f; //previous values are irrelevant
BramS23 0:df93928b266c 69
BramS23 0:df93928b266c 70 float Xcurr = 0.0f; //new value is calculated, old replaced
BramS23 0:df93928b266c 71 float Ycurr = 0.0f; //new value is calculated, old replaced
BramS23 0:df93928b266c 72
BramS23 0:df93928b266c 73 float b = 200.0f; //damping
BramS23 0:df93928b266c 74 float k = 0.05f; //stiffness of the spring pulling on the end effector
BramS23 0:df93928b266c 75
BramS23 0:df93928b266c 76 // Call function to calculate Xcurr and Ycurr from q1 and q2
BramS23 0:df93928b266c 77 Brocket(q1, q2, Xcurr, Ycurr);
BramS23 0:df93928b266c 78
BramS23 0:df93928b266c 79 // Compute spring forces
BramS23 0:df93928b266c 80 float Fx = k*(GXset-Xcurr);
BramS23 0:df93928b266c 81 float Fy = k*(GYset-Ycurr);
BramS23 0:df93928b266c 82
BramS23 0:df93928b266c 83 // Call function to calculate tau1 and tau2 from X,Ycurr and X,Yset
BramS23 0:df93928b266c 84 InverseJacobian(q1, q2, Fx, Fy, tau1, tau2);
BramS23 0:df93928b266c 85
BramS23 0:df93928b266c 86 // torque to joint velocity
BramS23 0:df93928b266c 87 float omg1 = tau1/b;
BramS23 0:df93928b266c 88 float omg2 = tau2/b;
BramS23 0:df93928b266c 89
BramS23 0:df93928b266c 90 // joint velocity to angles q1 and q2, where you define new q1 and q2 based on previous q1 and q2
BramS23 0:df93928b266c 91 q1set = q1set + omg1*Interval;
BramS23 0:df93928b266c 92 q2set = q2set + omg2*Interval;
BramS23 0:df93928b266c 93
BramS23 0:df93928b266c 94 // Call the function that controlls the motors
BramS23 0:df93928b266c 95 Motor1.GotoPos(q1set);
BramS23 0:df93928b266c 96 Motor2.GotoPos(q2set);
BramS23 0:df93928b266c 97 }
BramS23 0:df93928b266c 98
BramS23 0:df93928b266c 99
BramS23 0:df93928b266c 100 void LoopJacInverse(){
BramS23 0:df93928b266c 101 // Get Motor Positions
BramS23 0:df93928b266c 102 float q1 = Motor1.GetPos();
BramS23 0:df93928b266c 103 float q2 = Motor2.GetPos();
BramS23 0:df93928b266c 104
BramS23 0:df93928b266c 105 // Define velocities for control
BramS23 0:df93928b266c 106 float q1_dot=0.0f;
BramS23 0:df93928b266c 107 float q2_dot=0.0f;
BramS23 0:df93928b266c 108
BramS23 0:df93928b266c 109 // Get the velocities from EMG
BramS23 0:df93928b266c 110 float vx=0.0f;
BramS23 0:df93928b266c 111 float vy=0.0f;
BramS23 0:df93928b266c 112
BramS23 0:df93928b266c 113 // Apply Jacobian Inverse
BramS23 0:df93928b266c 114 InverseJacobian(q1,q2,vx,vy,q1_dot,q2_dot);
BramS23 0:df93928b266c 115
BramS23 0:df93928b266c 116 // Integrate q1dot and q2dot to obtain positions
BramS23 0:df93928b266c 117 q1set += q1_dot*Interval;
BramS23 0:df93928b266c 118 q2set += q2_dot*Interval;
BramS23 0:df93928b266c 119
BramS23 0:df93928b266c 120 // Call the motor control functions
BramS23 0:df93928b266c 121 Motor1.GotoPos(q1set);
BramS23 0:df93928b266c 122 Motor2.GotoPos(q2set);
BramS23 0:df93928b266c 123 }
BramS23 0:df93928b266c 124
BramS23 0:df93928b266c 125
BramS23 0:df93928b266c 126 // Start homing the motors
BramS23 0:df93928b266c 127 void HomingLoop(){
BramS23 0:df93928b266c 128 // with param:(Direction , PWM , Home pos in radians)
BramS23 0:df93928b266c 129 Motor1.Homing(1,0.2f,0.29f);
BramS23 0:df93928b266c 130 Motor2.Homing(1,0.1f,(3.3715f-2.0f*pi));
BramS23 0:df93928b266c 131 }
BramS23 0:df93928b266c 132
BramS23 0:df93928b266c 133 // Function for picking up a checkers playthingy
BramS23 0:df93928b266c 134 void PickUp(){
BramS23 0:df93928b266c 135
BramS23 0:df93928b266c 136 }
BramS23 0:df93928b266c 137
BramS23 0:df93928b266c 138 // Function for dropping a checkers playthingy
BramS23 0:df93928b266c 139 void LayDown(){
BramS23 0:df93928b266c 140
BramS23 0:df93928b266c 141 }
BramS23 0:df93928b266c 142
BramS23 0:df93928b266c 143 // Forward declarate remote controller function
BramS23 0:df93928b266c 144 void RemoteController();
BramS23 0:df93928b266c 145
BramS23 0:df93928b266c 146
BramS23 0:df93928b266c 147 // Give Reference Position
BramS23 0:df93928b266c 148 void DeterminePosRef(){
BramS23 0:df93928b266c 149 GXset=40*PotMeter1.read(); // Reference in Rad
BramS23 0:df93928b266c 150 GYset=40*PotMeter2.read(); // Reference in Rad
BramS23 0:df93928b266c 151 }
BramS23 0:df93928b266c 152
BramS23 0:df93928b266c 153 int main() {
BramS23 0:df93928b266c 154 pc.baud(115200);
BramS23 0:df93928b266c 155 pc.printf("Program BIOROBOTICS startup\r\n");
BramS23 0:df93928b266c 156
BramS23 0:df93928b266c 157 // Define Controller Values
BramS23 0:df93928b266c 158 Motor1.SetInputLimits(-2.0f*pi,2.0f*pi);
BramS23 0:df93928b266c 159 Motor1.SetInputLimits(-2.0f*pi,2.0f*pi);
BramS23 0:df93928b266c 160
BramS23 0:df93928b266c 161 Motor2.SetOutputLimits(-0.15f,0.15f);
BramS23 0:df93928b266c 162 Motor2.SetOutputLimits(-0.5f,0.5f);
BramS23 0:df93928b266c 163
BramS23 0:df93928b266c 164 Motor1.SetPID(100.0f,0.0f,0.001f);
BramS23 0:df93928b266c 165 Motor2.SetPID(100.0f,0.0f,0.001f);
BramS23 0:df93928b266c 166
BramS23 0:df93928b266c 167 Motor1.SetGearRatio(3.0f);
BramS23 0:df93928b266c 168 Motor2.SetGearRatio(1.8f);
BramS23 0:df93928b266c 169
BramS23 0:df93928b266c 170 // Start homing function
BramS23 0:df93928b266c 171 pc.printf("Homing \r\n");
BramS23 0:df93928b266c 172 HomingLoop();
BramS23 0:df93928b266c 173
BramS23 0:df93928b266c 174 // Start Tickers
BramS23 0:df93928b266c 175 pc.printf("Starting Tickers \r\n");
BramS23 0:df93928b266c 176 ControlTicker.attach(&LoopFunctionJacTransposed,Interval);
BramS23 0:df93928b266c 177 GetRefTicker.attach(&DeterminePosRef,0.5f);
BramS23 0:df93928b266c 178 DirectionButton.rise(&DirectionButtonPressed);
BramS23 0:df93928b266c 179
BramS23 0:df93928b266c 180 // Check wheater a remote command has been send
BramS23 0:df93928b266c 181 while(1)
BramS23 0:df93928b266c 182 {
BramS23 0:df93928b266c 183 if (ir_rx.getState() == ReceiverIR::Received)
BramS23 0:df93928b266c 184 {
BramS23 0:df93928b266c 185 pc.printf("received");
BramS23 0:df93928b266c 186
BramS23 0:df93928b266c 187
BramS23 0:df93928b266c 188 RemoteController();
BramS23 0:df93928b266c 189 wait(0.1);
BramS23 0:df93928b266c 190 }
BramS23 0:df93928b266c 191 }
BramS23 0:df93928b266c 192
BramS23 0:df93928b266c 193
BramS23 0:df93928b266c 194 while(1){}
BramS23 0:df93928b266c 195
BramS23 0:df93928b266c 196
BramS23 0:df93928b266c 197 }
BramS23 0:df93928b266c 198
BramS23 0:df93928b266c 199
BramS23 0:df93928b266c 200
BramS23 0:df93928b266c 201 void RemoteController(){
BramS23 0:df93928b266c 202
BramS23 0:df93928b266c 203 int bitcount;
BramS23 0:df93928b266c 204
BramS23 0:df93928b266c 205
BramS23 0:df93928b266c 206 bitcount = ir_rx.getData(&format, buf, sizeof(buf) * 8);
BramS23 0:df93928b266c 207 int state = buf[2];
BramS23 0:df93928b266c 208 switch(state)
BramS23 0:df93928b266c 209 {
BramS23 0:df93928b266c 210 case 22: //1
BramS23 0:df93928b266c 211 pc.printf("1\n\r");
BramS23 0:df93928b266c 212 break;
BramS23 0:df93928b266c 213 case 25: //1
BramS23 0:df93928b266c 214 pc.printf("2\n\r");
BramS23 0:df93928b266c 215 break;
BramS23 0:df93928b266c 216 case 13: //1
BramS23 0:df93928b266c 217 pc.printf("3\n\r");
BramS23 0:df93928b266c 218 break;
BramS23 0:df93928b266c 219 case 12: //1
BramS23 0:df93928b266c 220 pc.printf("4\n\r");
BramS23 0:df93928b266c 221 break;
BramS23 0:df93928b266c 222 case 24: //1
BramS23 0:df93928b266c 223 pc.printf("5\n\r");
BramS23 0:df93928b266c 224 ControlTicker.detach();
BramS23 0:df93928b266c 225 Motor1.Stop();
BramS23 0:df93928b266c 226 Motor2.Stop();
BramS23 0:df93928b266c 227 PickUp();
BramS23 0:df93928b266c 228 //ControlTicker.attach(&ControlLoop, Interval);
BramS23 0:df93928b266c 229 break;
BramS23 0:df93928b266c 230 case 94: //1
BramS23 0:df93928b266c 231 pc.printf("6\n\r");
BramS23 0:df93928b266c 232 break;
BramS23 0:df93928b266c 233 case 8: //1
BramS23 0:df93928b266c 234 pc.printf("7\n\r");
BramS23 0:df93928b266c 235 break;
BramS23 0:df93928b266c 236 case 28: //1
BramS23 0:df93928b266c 237 pc.printf("8\n\r");
BramS23 0:df93928b266c 238 ControlTicker.detach();
BramS23 0:df93928b266c 239 Motor1.Stop();
BramS23 0:df93928b266c 240 Motor2.Stop();
BramS23 0:df93928b266c 241 LayDown();
BramS23 0:df93928b266c 242 //ControlTicker.attach(&ControlLoop, Interval);
BramS23 0:df93928b266c 243 break;
BramS23 0:df93928b266c 244 case 90: //1
BramS23 0:df93928b266c 245 pc.printf("9\n\r");
BramS23 0:df93928b266c 246 break;
BramS23 0:df93928b266c 247 case 70: //1
BramS23 0:df93928b266c 248 pc.printf("Boven\n\r");
BramS23 0:df93928b266c 249 //PosRef2=PosRef2-0.1f;
BramS23 0:df93928b266c 250 /*
BramS23 0:df93928b266c 251 GYset = GYset + 1;
BramS23 0:df93928b266c 252 */
BramS23 0:df93928b266c 253 //pc.printf("%f\n\r", PosRef2);
BramS23 0:df93928b266c 254 break;
BramS23 0:df93928b266c 255 case 21: //1
BramS23 0:df93928b266c 256 pc.printf("Onder\n\r");
BramS23 0:df93928b266c 257 //PosRef2=PosRef2+0.1f;
BramS23 0:df93928b266c 258 /*
BramS23 0:df93928b266c 259 GYset = GYset - 1;
BramS23 0:df93928b266c 260 */
BramS23 0:df93928b266c 261 //pc.printf("%f\n\r", PosRef2);
BramS23 0:df93928b266c 262 break;
BramS23 0:df93928b266c 263 case 68: //1
BramS23 0:df93928b266c 264 pc.printf("Links\n\r");
BramS23 0:df93928b266c 265 //PosRef1=PosRef1+0.1f;
BramS23 0:df93928b266c 266 /*
BramS23 0:df93928b266c 267 GXset = GXset + 1;
BramS23 0:df93928b266c 268 */
BramS23 0:df93928b266c 269 //pc.printf("%f\n\r", PosRef1);
BramS23 0:df93928b266c 270 break;
BramS23 0:df93928b266c 271 case 67: //1
BramS23 0:df93928b266c 272 pc.printf("Rechts\n\r");
BramS23 0:df93928b266c 273 //PosRef1=PosRef1-0.1f;
BramS23 0:df93928b266c 274 /*
BramS23 0:df93928b266c 275 GXset = GXset - 1;
BramS23 0:df93928b266c 276 */
BramS23 0:df93928b266c 277 //pc.printf("%f\n\r", PosRef1);
BramS23 0:df93928b266c 278 break;
BramS23 0:df93928b266c 279 case 64: //1
BramS23 0:df93928b266c 280 pc.printf("OK\n\r");
BramS23 0:df93928b266c 281 //ControlTicker.detach();
BramS23 0:df93928b266c 282 //MotorThrottle1=0.0f;
BramS23 0:df93928b266c 283 //MotorThrottle2=0.0f;
BramS23 0:df93928b266c 284 //HomingLoop();
BramS23 0:df93928b266c 285 //ControlTicker.attach(&ControlLoop, Interval);
BramS23 0:df93928b266c 286
BramS23 0:df93928b266c 287 break;
BramS23 0:df93928b266c 288 default:
BramS23 0:df93928b266c 289 break;
BramS23 0:df93928b266c 290 }
BramS23 0:df93928b266c 291 }