sangbae Kim / Mbed 2 deprecated BilateralTeleoperation

Teleoperation code

Dependencies:   QEI mbed

main.cpp@0:c4bd1a2d961e, 2022-09-26 (annotated)

Committer:
sangbae
Date:
Mon Sep 26 19:40:10 2022 +0000
Revision:
0:c4bd1a2d961e
Teleoperation code for two motors using impedance control

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sangbae 0:c4bd1a2d961e 1 //Ben Katz, September 2013
sangbae 0:c4bd1a2d961e 2
sangbae 0:c4bd1a2d961e 3 #include "mbed.h"
sangbae 0:c4bd1a2d961e 4 #include "QEI.h"
sangbae 0:c4bd1a2d961e 5 #include <fstream>
sangbae 0:c4bd1a2d961e 6 #include <iomanip>
sangbae 0:c4bd1a2d961e 7 //Declaring pins
sangbae 0:c4bd1a2d961e 8 LocalFileSystem local("local");
sangbae 0:c4bd1a2d961e 9 Timer t;
sangbae 0:c4bd1a2d961e 10 float analogSignal;
sangbae 0:c4bd1a2d961e 11 float Angle1;
sangbae 0:c4bd1a2d961e 12 float preAngle1=0;
sangbae 0:c4bd1a2d961e 13 float Angle2;
sangbae 0:c4bd1a2d961e 14 float preAngle2=0;
sangbae 0:c4bd1a2d961e 15
sangbae 0:c4bd1a2d961e 16 float mCurrent1;
sangbae 0:c4bd1a2d961e 17 float mCurrent2;
sangbae 0:c4bd1a2d961e 18 float dCurrent1;
sangbae 0:c4bd1a2d961e 19 float dCurrent2;
sangbae 0:c4bd1a2d961e 20 float dAngle1;
sangbae 0:c4bd1a2d961e 21 float dAngle2;
sangbae 0:c4bd1a2d961e 22 float dAngularVelocity1=0;
sangbae 0:c4bd1a2d961e 23 float dAngularVelocity2=0;
sangbae 0:c4bd1a2d961e 24 float AngularVelocity1;
sangbae 0:c4bd1a2d961e 25 float AngularVelocity2;
sangbae 0:c4bd1a2d961e 26 float duty1;
sangbae 0:c4bd1a2d961e 27 float duty2;
sangbae 0:c4bd1a2d961e 28 float alpha;
sangbae 0:c4bd1a2d961e 29 float CurrentControlPeriod;
sangbae 0:c4bd1a2d961e 30 float DataSavingPeriod=0.005;
sangbae 0:c4bd1a2d961e 31
sangbae 0:c4bd1a2d961e 32 float Kp=5;
sangbae 0:c4bd1a2d961e 33 float Kd=0.04;
sangbae 0:c4bd1a2d961e 34 float Kv=0.35;
sangbae 0:c4bd1a2d961e 35 float R=2.2;
sangbae 0:c4bd1a2d961e 36 float Ki=5;
sangbae 0:c4bd1a2d961e 37 DigitalOut dOut1(p5);
sangbae 0:c4bd1a2d961e 38 DigitalOut dOut2(p7); //pin 5 set as a digital output Pins 5-30 can be used as digital I/O.
sangbae 0:c4bd1a2d961e 39 DigitalOut dOut3(p8);
sangbae 0:c4bd1a2d961e 40 DigitalOut dOut4(p9);
sangbae 0:c4bd1a2d961e 41 //pin 6 set as digital input.
sangbae 0:c4bd1a2d961e 42 AnalogIn aIn1(p16);
sangbae 0:c4bd1a2d961e 43 AnalogIn aIn2(p17); //pin 15 set as analog input. Pins 15-20 can be used as analog inputs.
sangbae 0:c4bd1a2d961e 44 PwmOut pwmOut1(p21);
sangbae 0:c4bd1a2d961e 45 PwmOut pwmOut2(p22); //pin 21 set as PWM output. Pins 21-26 can be used as PWM outputs.
sangbae 0:c4bd1a2d961e 46 QEI motor1(p27, p28, NC, 1200, QEI::X4_ENCODING);
sangbae 0:c4bd1a2d961e 47 QEI motor2(p23, p24, NC, 1200, QEI::X4_ENCODING);
sangbae 0:c4bd1a2d961e 48 //ofstream out;
sangbae 0:c4bd1a2d961e 49 FILE *fp= fopen("/local/data.csv","w");
sangbae 0:c4bd1a2d961e 50
sangbae 0:c4bd1a2d961e 51 //Serial pc(USBTX, USBRX); //create a serial connection to the computer over the tx/rx pins
sangbae 0:c4bd1a2d961e 52
sangbae 0:c4bd1a2d961e 53 void Control()
sangbae 0:c4bd1a2d961e 54 {
sangbae 0:c4bd1a2d961e 55 dAngle1 =motor2.getPulses()/1200.0*6.283;;
sangbae 0:c4bd1a2d961e 56 dAngularVelocity1=AngularVelocity2;
sangbae 0:c4bd1a2d961e 57 preAngle1=Angle1;
sangbae 0:c4bd1a2d961e 58 Angle1 = motor1.getPulses();
sangbae 0:c4bd1a2d961e 59 Angle1 =Angle1/1200.0*6.283;
sangbae 0:c4bd1a2d961e 60 AngularVelocity1 =(Angle1-preAngle1)/CurrentControlPeriod;
sangbae 0:c4bd1a2d961e 61 mCurrent1 = aIn1.read()*3.3/.14;
sangbae 0:c4bd1a2d961e 62
sangbae 0:c4bd1a2d961e 63 dCurrent1 = 3*(dAngle1-Angle1)+0.04*(dAngularVelocity1-AngularVelocity1)
sangbae 0:c4bd1a2d961e 64
sangbae 0:c4bd1a2d961e 65 if (dCurrent1>0) {
sangbae 0:c4bd1a2d961e 66 dOut1=1;
sangbae 0:c4bd1a2d961e 67 dOut2=0;
sangbae 0:c4bd1a2d961e 68 duty1= (abs(dCurrent1)*2.10+0.35*abs(AngularVelocity1)+5*(abs(dCurrent1)-mCurrent1))/12.0;
sangbae 0:c4bd1a2d961e 69 } else if (dCurrent1<0) {
sangbae 0:c4bd1a2d961e 70 dOut1=0;
sangbae 0:c4bd1a2d961e 71 dOut2=1;
sangbae 0:c4bd1a2d961e 72 duty1= (abs(dCurrent1)*2.10-0.35*abs(AngularVelocity1)+5*(abs(dCurrent1)-mCurrent1))/12.0;
sangbae 0:c4bd1a2d961e 73 }
sangbae 0:c4bd1a2d961e 74
sangbae 0:c4bd1a2d961e 75 if (duty1>0.99) {
sangbae 0:c4bd1a2d961e 76 duty1 =1;
sangbae 0:c4bd1a2d961e 77 }
sangbae 0:c4bd1a2d961e 78
sangbae 0:c4bd1a2d961e 79 pwmOut1.write(duty1);
sangbae 0:c4bd1a2d961e 80 // Convert to Radians
sangbae 0:c4bd1a2d961e 81
sangbae 0:c4bd1a2d961e 82
sangbae 0:c4bd1a2d961e 83
sangbae 0:c4bd1a2d961e 84
sangbae 0:c4bd1a2d961e 85
sangbae 0:c4bd1a2d961e 86 dAngle2 =motor1.getPulses()/1200.0*6.283;;
sangbae 0:c4bd1a2d961e 87 dAngularVelocity1=AngularVelocity2;
sangbae 0:c4bd1a2d961e 88 preAngle2=Angle2;
sangbae 0:c4bd1a2d961e 89 Angle2 = motor2.getPulses();
sangbae 0:c4bd1a2d961e 90 Angle2 =Angle2/1200.0*6.283;
sangbae 0:c4bd1a2d961e 91 AngularVelocity2 =(Angle2-preAngle2)/CurrentControlPeriod;
sangbae 0:c4bd1a2d961e 92 mCurrent2 = aIn2.read()*3.3/.14;
sangbae 0:c4bd1a2d961e 93
sangbae 0:c4bd1a2d961e 94 dCurrent2 = 3*(dAngle2-Angle2)+0.04*(dAngularVelocity2-AngularVelocity2); //0.016-p gain 0.04- D gain
sangbae 0:c4bd1a2d961e 95
sangbae 0:c4bd1a2d961e 96 if (dCurrent2>0) {
sangbae 0:c4bd1a2d961e 97 dOut3=1;
sangbae 0:c4bd1a2d961e 98 dOut4=0;
sangbae 0:c4bd1a2d961e 99 duty2= (abs(dCurrent2)*2.10+0.35*abs(AngularVelocity2)+5*(abs(dCurrent2)-mCurrent2))/12.0;
sangbae 0:c4bd1a2d961e 100 } else if (dCurrent2<0) {
sangbae 0:c4bd1a2d961e 101 dOut3=0;
sangbae 0:c4bd1a2d961e 102 dOut4=1;
sangbae 0:c4bd1a2d961e 103 duty2= (abs(dCurrent2)*2.10-0.35*abs(AngularVelocity2)+5*(abs(dCurrent2)-mCurrent2))/12.0;
sangbae 0:c4bd1a2d961e 104 }
sangbae 0:c4bd1a2d961e 105
sangbae 0:c4bd1a2d961e 106 if (duty2>0.99) {
sangbae 0:c4bd1a2d961e 107 duty2 =1;
sangbae 0:c4bd1a2d961e 108 }
sangbae 0:c4bd1a2d961e 109
sangbae 0:c4bd1a2d961e 110 pwmOut2.write(duty2);
sangbae 0:c4bd1a2d961e 111
sangbae 0:c4bd1a2d961e 112
sangbae 0:c4bd1a2d961e 113
sangbae 0:c4bd1a2d961e 114 }
sangbae 0:c4bd1a2d961e 115
sangbae 0:c4bd1a2d961e 116 void Save()
sangbae 0:c4bd1a2d961e 117 {
sangbae 0:c4bd1a2d961e 118 fprintf(fp, "%F,%F\n",mCurrent1,Angle1);
sangbae 0:c4bd1a2d961e 119 }
sangbae 0:c4bd1a2d961e 120
sangbae 0:c4bd1a2d961e 121
sangbae 0:c4bd1a2d961e 122
sangbae 0:c4bd1a2d961e 123 Ticker control;
sangbae 0:c4bd1a2d961e 124 Ticker save;
sangbae 0:c4bd1a2d961e 125 int main()
sangbae 0:c4bd1a2d961e 126 {
sangbae 0:c4bd1a2d961e 127 alpha=0.0;
sangbae 0:c4bd1a2d961e 128 CurrentControlPeriod=0.001;
sangbae 0:c4bd1a2d961e 129 DataSavingPeriod=0.005;
sangbae 0:c4bd1a2d961e 130
sangbae 0:c4bd1a2d961e 131 pwmOut1.period(0.0001);
sangbae 0:c4bd1a2d961e 132 pwmOut2.period(0.0001);
sangbae 0:c4bd1a2d961e 133
sangbae 0:c4bd1a2d961e 134 //Set PWM output freqency to 20 kHz
sangbae 0:c4bd1a2d961e 135 control.attach(&Control,CurrentControlPeriod);
sangbae 0:c4bd1a2d961e 136
sangbae 0:c4bd1a2d961e 137 save.attach(&Save,DataSavingPeriod);
sangbae 0:c4bd1a2d961e 138 wait (1);
sangbae 0:c4bd1a2d961e 139
sangbae 0:c4bd1a2d961e 140
sangbae 0:c4bd1a2d961e 141 // fclose(fp);
sangbae 0:c4bd1a2d961e 142
sangbae 0:c4bd1a2d961e 143 }