M9 Biorobotics Group 8 / Mbed 2 deprecated motorcalibration_StateMasjien

Moet dit er bij

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@8:3cfc8be293d3, 2019-10-29 (annotated)

Committer:
BasB
Date:
Tue Oct 29 16:42:46 2019 +0000
Revision:
8:3cfc8be293d3
Parent:
7:676a83def149
Child:
9:298469a70280
Werkende calibratie [handmatig]

Who changed what in which revision?

UserRevisionLine numberNew contents of line
BasB 0:335646ab45c0 1 #include "mbed.h"
BasB 0:335646ab45c0 2 #include "HIDScope.h"
BasB 0:335646ab45c0 3 #include "QEI.h"
BasB 0:335646ab45c0 4 #include "MODSERIAL.h"
BasB 0:335646ab45c0 5 #include "BiQuad.h"
BasB 0:335646ab45c0 6 #include "FastPWM.h"
BasB 0:335646ab45c0 7
BasB 0:335646ab45c0 8 // Button and potmeter1 control
BasB 0:335646ab45c0 9 InterruptIn button1(D11);
BasB 0:335646ab45c0 10 InterruptIn button2(D10);
BasB 0:335646ab45c0 11 InterruptIn buttonsw2(SW2);
BasB 0:335646ab45c0 12 InterruptIn buttonsw3(SW3);
BasB 0:335646ab45c0 13 AnalogIn potmeter1(A0);
BasB 0:335646ab45c0 14 AnalogIn potmeter2(A1);
BasB 0:335646ab45c0 15 AnalogIn potmeter3(A2);
BasB 0:335646ab45c0 16 AnalogIn potmeter4(A3);
BasB 2:7ea5ae2287a7 17
BasB 0:335646ab45c0 18 // Encoder
BasB 0:335646ab45c0 19 DigitalIn encA1(D9);
BasB 0:335646ab45c0 20 DigitalIn encB1(D8);
BasB 0:335646ab45c0 21 DigitalIn encA2(D13);
BasB 0:335646ab45c0 22 DigitalIn encB2(D13);
BasB 0:335646ab45c0 23 QEI encoder1(D9,D8,NC,64,QEI::X4_ENCODING); //Encoding motor 1
BasB 0:335646ab45c0 24 QEI encoder2(D13,D12,NC,64,QEI::X4_ENCODING); //Encoding motor 2
BasB 0:335646ab45c0 25 float Ts = 0.01; //Sample time
BasB 0:335646ab45c0 26 float motor1angle; //Measured angle motor 1
BasB 0:335646ab45c0 27 float motor2angle; //Measured angle motor 2
BasB 7:676a83def149 28 float motor1offset; //Offset bij calibratie
BasB 7:676a83def149 29 float motor2offset;
BasB 2:7ea5ae2287a7 30 float potmeter;
BasB 4:1e8da6b5f147 31 float omega1; //velocity rad/s motor 1
BasB 0:335646ab45c0 32 float omega2; //Velocity rad/s motor2
BasB 0:335646ab45c0 33 float deg2rad=0.0174532; //Conversion factor degree to rad
BasB 0:335646ab45c0 34 float rad2deg=57.29578; //Conversion factor rad to degree
BasB 0:335646ab45c0 35
BasB 0:335646ab45c0 36
BasB 0:335646ab45c0 37 // Motor
BasB 0:335646ab45c0 38 DigitalOut motor2Direction(D4);
BasB 0:335646ab45c0 39 FastPWM motor2Power(D5);
BasB 0:335646ab45c0 40 DigitalOut motor1Direction(D7);
BasB 0:335646ab45c0 41 FastPWM motor1Power(D6);
BasB 0:335646ab45c0 42
BasB 0:335646ab45c0 43 //Motorcontrol
BasB 0:335646ab45c0 44 bool motordir1;
BasB 0:335646ab45c0 45 bool motordir2;
BasB 0:335646ab45c0 46 float motor1ref=0.1745;
BasB 0:335646ab45c0 47 float motor2ref=0.0873;
BasB 0:335646ab45c0 48 double controlsignal1;
BasB 0:335646ab45c0 49 double controlsignal2;
BasB 0:335646ab45c0 50 double pi2= 6.283185;
BasB 0:335646ab45c0 51 float motor1error; //motor 1 error
BasB 0:335646ab45c0 52 float motor2error;
BasB 0:335646ab45c0 53 float Kp=0.27;
BasB 0:335646ab45c0 54 float Ki=0.35;
BasB 0:335646ab45c0 55 float Kd=0.1;
BasB 0:335646ab45c0 56 float u_p1;
BasB 0:335646ab45c0 57 float u_p2;
BasB 0:335646ab45c0 58 float u_i1;
BasB 0:335646ab45c0 59 float u_i2;
BasB 0:335646ab45c0 60
BasB 0:335646ab45c0 61 //Windup control
BasB 0:335646ab45c0 62 float ux1;
BasB 0:335646ab45c0 63 float ux2;
BasB 0:335646ab45c0 64 float up1; //Proportional contribution motor 1
BasB 0:335646ab45c0 65 float up2; //Proportional contribution motor 2
BasB 0:335646ab45c0 66 float ek1;
BasB 0:335646ab45c0 67 float ek2;
BasB 0:335646ab45c0 68 float ei1= 0.0; //Error integral motor 1
BasB 0:335646ab45c0 69 float ei2=0.0; //Error integral motor 2
BasB 0:335646ab45c0 70 float Ka= 1.0; //Integral windup gain
BasB 0:335646ab45c0 71
BasB 0:335646ab45c0 72 //RKI
BasB 0:335646ab45c0 73 float Vx=0.0; //Desired linear velocity x direction
BasB 0:335646ab45c0 74 float Vy=0.0; //Desired linear velocity y direction
BasB 0:335646ab45c0 75 float q1=0.0f*deg2rad; //Angle of first joint [rad]
BasB 0:335646ab45c0 76 float q2=-135.0f*deg2rad; //Angle of second joint [rad]
BasB 0:335646ab45c0 77 float q1dot; //Velocity of first joint [rad/s]
BasB 0:335646ab45c0 78 float q2dot; //Velocity of second joint [rad/s]
BasB 0:335646ab45c0 79 float l1=26.0; //Distance base-link [cm]
BasB 0:335646ab45c0 80 float l2=62.0; //Distance link-endpoint [cm]
BasB 0:335646ab45c0 81 float xe; //Endpoint x position [cm]
BasB 0:335646ab45c0 82 float ye; //Endpoint y position [cm]
BasB 0:335646ab45c0 83
BasB 0:335646ab45c0 84 //Hidscope
BasB 0:335646ab45c0 85 HIDScope scope(6); //Going to send x channels of data. To access data go to 'http:/localhost:18082/' after starting HIDScope application.
BasB 0:335646ab45c0 86
BasB 3:6e28b992b99e 87 //State maschine
BasB 8:3cfc8be293d3 88 enum Motor_States{motor_wait , motor_encoderset};
BasB 3:6e28b992b99e 89 Motor_States motor_curr_state;
BasB 3:6e28b992b99e 90 bool motor_state_changed = true;
BasB 3:6e28b992b99e 91 bool motor_cal1_done = false;
BasB 3:6e28b992b99e 92 bool motor_cal2_done = false;
BasB 3:6e28b992b99e 93
BasB 3:6e28b992b99e 94 bool button1_pressed = false;
BasB 3:6e28b992b99e 95 bool button2_pressed = false;
BasB 3:6e28b992b99e 96
BasB 0:335646ab45c0 97 // PC connection
BasB 0:335646ab45c0 98 MODSERIAL pc(USBTX, USBRX);
BasB 0:335646ab45c0 99
BasB 0:335646ab45c0 100 // Intializing tickers
BasB 0:335646ab45c0 101 Ticker motorTicker;
BasB 0:335646ab45c0 102 Ticker controlTicker;
BasB 0:335646ab45c0 103 Ticker directionTicker;
BasB 0:335646ab45c0 104 Ticker encoderTicker;
BasB 0:335646ab45c0 105 Ticker scopeTicker;
BasB 3:6e28b992b99e 106 Ticker tickGlobal; //Global ticker
BasB 0:335646ab45c0 107
BasB 0:335646ab45c0 108 const float PWM_period = 1e-6;
BasB 0:335646ab45c0 109
BasB 0:335646ab45c0 110 volatile int counts1; // Encoder counts
BasB 0:335646ab45c0 111 volatile int counts2;
BasB 0:335646ab45c0 112 volatile int countsPrev1 = 0;
BasB 0:335646ab45c0 113 volatile int countsPrev2 = 0;
BasB 0:335646ab45c0 114 volatile int deltaCounts1;
BasB 0:335646ab45c0 115 volatile int deltaCounts2;
BasB 0:335646ab45c0 116
BasB 0:335646ab45c0 117 float factorin = 6.23185/64; // Convert encoder counts to angle in rad
BasB 0:335646ab45c0 118 float gearratio = 131.25; // Gear ratio of gearbox
BasB 0:335646ab45c0 119
BasB 3:6e28b992b99e 120 void button1Press()
BasB 3:6e28b992b99e 121 {
BasB 3:6e28b992b99e 122 button1_pressed = true;
BasB 3:6e28b992b99e 123 }
BasB 0:335646ab45c0 124
BasB 6:e7e39d116ed0 125 void button2Press()
BasB 6:e7e39d116ed0 126 {
BasB 6:e7e39d116ed0 127 button2_pressed = true;
BasB 6:e7e39d116ed0 128 }
BasB 0:335646ab45c0 129
BasB 3:6e28b992b99e 130 // Ticker Functions
BasB 0:335646ab45c0 131 void readEncoder()
BasB 0:335646ab45c0 132 {
BasB 0:335646ab45c0 133 counts1 = encoder1.getPulses();
BasB 0:335646ab45c0 134 deltaCounts1 = counts1 - countsPrev1;
BasB 0:335646ab45c0 135 countsPrev1 = counts1;
BasB 0:335646ab45c0 136
BasB 0:335646ab45c0 137 counts2 = encoder2.getPulses();
BasB 0:335646ab45c0 138 deltaCounts2 = counts2 - countsPrev2;
BasB 0:335646ab45c0 139 countsPrev2 = counts2;
BasB 0:335646ab45c0 140 }
BasB 0:335646ab45c0 141
BasB 8:3cfc8be293d3 142 void do_motor_wait(){
BasB 3:6e28b992b99e 143 // Entry function
BasB 3:6e28b992b99e 144 if ( motor_state_changed == true ) {
BasB 3:6e28b992b99e 145 motor_state_changed = false;
BasB 3:6e28b992b99e 146 // More functions
BasB 3:6e28b992b99e 147 }
BasB 3:6e28b992b99e 148
BasB 8:3cfc8be293d3 149 // Do nothing until end condition is met
BasB 5:4d8b85b7cfc4 150
BasB 8:3cfc8be293d3 151 // State transition guard
BasB 8:3cfc8be293d3 152 if ( button2_pressed ) {
BasB 8:3cfc8be293d3 153 button2_pressed = false;
BasB 8:3cfc8be293d3 154 motor_curr_state = motor_encoderset; //Beginnen met calibratie
BasB 3:6e28b992b99e 155 motor_state_changed = true;
BasB 3:6e28b992b99e 156 // More functions
BasB 3:6e28b992b99e 157 }
BasB 8:3cfc8be293d3 158
BasB 8:3cfc8be293d3 159 }
BasB 0:335646ab45c0 160
BasB 3:6e28b992b99e 161 void do_motor_Encoder_Set(){
BasB 3:6e28b992b99e 162 // Entry function
BasB 3:6e28b992b99e 163 if ( motor_state_changed == true ) {
BasB 3:6e28b992b99e 164 motor_state_changed = false;
BasB 3:6e28b992b99e 165 // More functions
BasB 3:6e28b992b99e 166 }
BasB 7:676a83def149 167 motor1Power.write(0.0f);
BasB 7:676a83def149 168 motor2Power.write(0.0f);
BasB 7:676a83def149 169 motor1offset = (counts1 * factorin / gearratio);
BasB 7:676a83def149 170 motor2offset = (counts2 * factorin / gearratio);
BasB 7:676a83def149 171
BasB 3:6e28b992b99e 172 // State transition guard
BasB 7:676a83def149 173 if ( button2_pressed ) {
BasB 7:676a83def149 174 button2_pressed = false;
BasB 7:676a83def149 175 motor_curr_state = motor_wait;
BasB 3:6e28b992b99e 176 motor_state_changed = true;
BasB 3:6e28b992b99e 177 }
BasB 3:6e28b992b99e 178 }
BasB 3:6e28b992b99e 179
BasB 3:6e28b992b99e 180 void motor_state_machine()
BasB 3:6e28b992b99e 181 {
BasB 3:6e28b992b99e 182 switch(motor_curr_state) {
BasB 3:6e28b992b99e 183 case motor_wait:
BasB 3:6e28b992b99e 184 do_motor_wait();
BasB 3:6e28b992b99e 185 break;
BasB 3:6e28b992b99e 186 case motor_encoderset:
BasB 3:6e28b992b99e 187 do_motor_Encoder_Set();
BasB 3:6e28b992b99e 188 break;
BasB 3:6e28b992b99e 189 }
BasB 3:6e28b992b99e 190 }
BasB 3:6e28b992b99e 191
BasB 3:6e28b992b99e 192 // Global loop of program
BasB 3:6e28b992b99e 193 void tickGlobalFunc()
BasB 3:6e28b992b99e 194 {
BasB 3:6e28b992b99e 195 //sampleSignal();
BasB 3:6e28b992b99e 196 //emg_state_machine();
BasB 3:6e28b992b99e 197 motor_state_machine();
BasB 4:1e8da6b5f147 198 readEncoder();
BasB 3:6e28b992b99e 199 // controller();
BasB 3:6e28b992b99e 200 // outputToMotors();
BasB 3:6e28b992b99e 201 }
BasB 3:6e28b992b99e 202
BasB 3:6e28b992b99e 203
BasB 0:335646ab45c0 204 int main()
BasB 0:335646ab45c0 205 {
BasB 0:335646ab45c0 206 pc.baud(115200);
BasB 0:335646ab45c0 207 pc.printf("\r\nStarting...\r\n\r\n");
BasB 0:335646ab45c0 208 motor1Power.period(PWM_period);
BasB 0:335646ab45c0 209 motor2Power.period(PWM_period);
BasB 2:7ea5ae2287a7 210
BasB 3:6e28b992b99e 211 motor_curr_state = motor_wait; // Start off in EMG Wait state
BasB 3:6e28b992b99e 212 tickGlobal.attach( &tickGlobalFunc, Ts );
BasB 0:335646ab45c0 213
BasB 4:1e8da6b5f147 214 button1.fall(&button1Press);
BasB 6:e7e39d116ed0 215 button2.fall(&button2Press);
BasB 0:335646ab45c0 216
BasB 0:335646ab45c0 217 while (true) {
BasB 2:7ea5ae2287a7 218 pc.printf("Omega1: %f Omega 2: %f controlsignal1: %f \r\n", omega1, omega2, controlsignal1);
BasB 7:676a83def149 219 pc.printf("Currentstate: %i\r\n",motor_curr_state);
BasB 7:676a83def149 220 pc.printf("motor1offset: %f motor2offset: %f\r\n",motor1offset,motor2offset);
BasB 0:335646ab45c0 221 wait(0.5);
BasB 0:335646ab45c0 222 }
BasB 0:335646ab45c0 223 }