Eki Liptiay / Mbed 2 deprecated PseudoScript

ja

Dependencies:   mbed

main.cpp@0:8f027052f23a, 2018-10-30 (annotated)

Committer:
ekiliptiay
Date:
Tue Oct 30 11:39:57 2018 +0000
Revision:
0:8f027052f23a
Pseudo;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ekiliptiay 0:8f027052f23a 1 #include "mbed.h"
ekiliptiay 0:8f027052f23a 2
ekiliptiay 0:8f027052f23a 3 // Load all the necessary pins, buttons, etc.
ekiliptiay 0:8f027052f23a 4
ekiliptiay 0:8f027052f23a 5 // Load all the EMGs
ekiliptiay 0:8f027052f23a 6 AnalogIn EMG1; // Move X
ekiliptiay 0:8f027052f23a 7 AnalogIn EMG2; // Move Y
ekiliptiay 0:8f027052f23a 8 AnalogIn EMG3; // Change direction
ekiliptiay 0:8f027052f23a 9
ekiliptiay 0:8f027052f23a 10 // Initiate motor 1
ekiliptiay 0:8f027052f23a 11 PwmOut pwmpin1(D6);
ekiliptiay 0:8f027052f23a 12 DigitalOut directionpin1(D7);
ekiliptiay 0:8f027052f23a 13 InterruptIn motor1A(D12); // Encoder 1a
ekiliptiay 0:8f027052f23a 14 InterruptIn motor1B(D13); // Encoder 1b
ekiliptiay 0:8f027052f23a 15 QEI Encoder1(D12,D13,NC,64); // Note that we use X2 encoding, so there are 4200 counts/rotation, as opposed to 8400 from the documentation.
ekiliptiay 0:8f027052f23a 16
ekiliptiay 0:8f027052f23a 17 // Initiate motor 2
ekiliptiay 0:8f027052f23a 18 DigitalOut directionpin2(D4);
ekiliptiay 0:8f027052f23a 19 PwmOut pwmpin2(D5);
ekiliptiay 0:8f027052f23a 20 InterruptIn motor2A(D10); // Encoder 1a
ekiliptiay 0:8f027052f23a 21 InterruptIn motor2B(D11); // Encoder 1b
ekiliptiay 0:8f027052f23a 22 QEI Encoder2(D10,D11,NC,64); // Note that we use X2 encoding, so there are 4200 counts/rotation, as opposed to 8400 from the documentation.
ekiliptiay 0:8f027052f23a 23
ekiliptiay 0:8f027052f23a 24 // -------------------------------------------------------------------------- \\
ekiliptiay 0:8f027052f23a 25
ekiliptiay 0:8f027052f23a 26 double FilterEMG1(EMG1){
ekiliptiay 0:8f027052f23a 27 filter emg1
ekiliptiay 0:8f027052f23a 28 return filteredemg1
ekiliptiay 0:8f027052f23a 29 }
ekiliptiay 0:8f027052f23a 30
ekiliptiay 0:8f027052f23a 31 double FilterEMG2(EMG2){
ekiliptiay 0:8f027052f23a 32 filter emg2
ekiliptiay 0:8f027052f23a 33 return filteredemg2
ekiliptiay 0:8f027052f23a 34 }
ekiliptiay 0:8f027052f23a 35
ekiliptiay 0:8f027052f23a 36 double FilterEMG3(EMG3){
ekiliptiay 0:8f027052f23a 37 filter emg3
ekiliptiay 0:8f027052f23a 38 return filteredemg3
ekiliptiay 0:8f027052f23a 39 }
ekiliptiay 0:8f027052f23a 40
ekiliptiay 0:8f027052f23a 41 // ----------------------Read & relate the emgs --------------------------- \\
ekiliptiay 0:8f027052f23a 42
ekiliptiay 0:8f027052f23a 43 // We want to know whether we should move X or Y
ekiliptiay 0:8f027052f23a 44 bool DirBool = 1; // initialise with moving X
ekiliptiay 0:8f027052f23a 45 void ChangeDir(emg3 lezen){
ekiliptiay 0:8f027052f23a 46 // Read EMG 3
ekiliptiay 0:8f027052f23a 47 if(voldoet emg3 aan de voorwaarde? verander dan de x/y bool.){
ekiliptiay 0:8f027052f23a 48 DirBool = !DirBool;
ekiliptiay 0:8f027052f23a 49 return DirBool;
ekiliptiay 0:8f027052f23a 50 }
ekiliptiay 0:8f027052f23a 51
ekiliptiay 0:8f027052f23a 52 // Start moving in the x-direction
ekiliptiay 0:8f027052f23a 53 void MoveX(bool DirBool, EMG1){
ekiliptiay 0:8f027052f23a 54 // first, we check whether or not we should even move in the x-direction
ekiliptiay 0:8f027052f23a 55 if(DirBool == 1){ // We can move in the x-direction if this is true
ekiliptiay 0:8f027052f23a 56 // we make a list of 2 long, checking emg 1 and 2
ekiliptiay 0:8f027052f23a 57 *list*
ekiliptiay 0:8f027052f23a 58 check whether emg 1 is active or not
ekiliptiay 0:8f027052f23a 59 if(emg1 voldoet aan statement){
ekiliptiay 0:8f027052f23a 60 request a change in motor angle // could be done by saying motorangle = motorangle - something we want, check convention!!!
ekiliptiay 0:8f027052f23a 61 }
ekiliptiay 0:8f027052f23a 62 if(emg2 voldoet aan statement){
ekiliptiay 0:8f027052f23a 63 request a change in motor angle // could be done by saying motorangle = motorangle - something we want, check convention!!!
ekiliptiay 0:8f027052f23a 64 }
ekiliptiay 0:8f027052f23a 65 return de angle die we willen veranderen voor de motoren
ekiliptiay 0:8f027052f23a 66 }
ekiliptiay 0:8f027052f23a 67
ekiliptiay 0:8f027052f23a 68 // When we should move, we should relate this change in x-direction to a change
ekiliptiay 0:8f027052f23a 69 // in the motor angles, this comes from the inverse kinematics.
ekiliptiay 0:8f027052f23a 70
ekiliptiay 0:8f027052f23a 71 void MoveY(bool DirBool, EMG1){
ekiliptiay 0:8f027052f23a 72 // first, we check whether or not we should even move in the y-direction
ekiliptiay 0:8f027052f23a 73 if(DirBool == 0){ // We can move in the y-direction if this is true
ekiliptiay 0:8f027052f23a 74 // we make a list of 2 long, checking emg 1 and 2
ekiliptiay 0:8f027052f23a 75 *list*
ekiliptiay 0:8f027052f23a 76 check whether emg 1 is active or not
ekiliptiay 0:8f027052f23a 77 if(emg1 voldoet aan statement){
ekiliptiay 0:8f027052f23a 78 request a change in motor angle // could be done by saying motorangle = motorangle - something we want, check convention!!!
ekiliptiay 0:8f027052f23a 79 }
ekiliptiay 0:8f027052f23a 80 if(emg2 voldoet aan statement){
ekiliptiay 0:8f027052f23a 81 request a change in motor angle // could be done by saying motorangle = motorangle - something we want, check convention!!!
ekiliptiay 0:8f027052f23a 82 }
ekiliptiay 0:8f027052f23a 83 return de angle die we willen veranderen voor de motoren
ekiliptiay 0:8f027052f23a 84 }
ekiliptiay 0:8f027052f23a 85
ekiliptiay 0:8f027052f23a 86 // When we should move, we should relate this change in y-direction to a change
ekiliptiay 0:8f027052f23a 87 // in the motor angles, this comes from the inverse kinematics.
ekiliptiay 0:8f027052f23a 88
ekiliptiay 0:8f027052f23a 89 // ----------Set the desired motor positions & get the actual ones---------- \\
ekiliptiay 0:8f027052f23a 90
ekiliptiay 0:8f027052f23a 91 // Get position motor 1 should be in
ekiliptiay 0:8f027052f23a 92 double GetReferencePosition1(){
ekiliptiay 0:8f027052f23a 93 return ReferencePosition1;
ekiliptiay 0:8f027052f23a 94 }
ekiliptiay 0:8f027052f23a 95
ekiliptiay 0:8f027052f23a 96 // Get position motor 1 actually is in
ekiliptiay 0:8f027052f23a 97 double GetActualPosition1(){
ekiliptiay 0:8f027052f23a 98 double ActualPosition = Encoder1.getPulses(); // Gets the actual amount of counts from motor 1. Note the minus sign for convention (CCW = positive counts.)
ekiliptiay 0:8f027052f23a 99 return ActualPosition1;
ekiliptiay 0:8f027052f23a 100 }
ekiliptiay 0:8f027052f23a 101
ekiliptiay 0:8f027052f23a 102 // Get position motor 2 should be in
ekiliptiay 0:8f027052f23a 103 double GetReferencePosition2(){
ekiliptiay 0:8f027052f23a 104 return ReferencePosition2;
ekiliptiay 0:8f027052f23a 105 }
ekiliptiay 0:8f027052f23a 106
ekiliptiay 0:8f027052f23a 107 // Get position motor 2 actually is in
ekiliptiay 0:8f027052f23a 108 double GetActualPosition2(){
ekiliptiay 0:8f027052f23a 109 double ActualPosition2 = Encoder2.getPulses(); // Gets the actual amount of counts from motor 1. Note the minus sign for convention (CCW = positive counts.)
ekiliptiay 0:8f027052f23a 110 return ActualPosition2;
ekiliptiay 0:8f027052f23a 111 }
ekiliptiay 0:8f027052f23a 112
ekiliptiay 0:8f027052f23a 113 // PID controller for motor 1
ekiliptiay 0:8f027052f23a 114 double PID_controller1(double error){
ekiliptiay 0:8f027052f23a 115 static double error_integral = 0;
ekiliptiay 0:8f027052f23a 116 static double error_prev = error; // initialization with this value only done once!
ekiliptiay 0:8f027052f23a 117 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
ekiliptiay 0:8f027052f23a 118 double Ts = 0.01;
ekiliptiay 0:8f027052f23a 119
ekiliptiay 0:8f027052f23a 120 // Proportional part
ekiliptiay 0:8f027052f23a 121
ekiliptiay 0:8f027052f23a 122 double Kp = 0.1;
ekiliptiay 0:8f027052f23a 123 double u_k = Kp*error;
ekiliptiay 0:8f027052f23a 124
ekiliptiay 0:8f027052f23a 125 // Integral part
ekiliptiay 0:8f027052f23a 126 double Ki = 0.1;
ekiliptiay 0:8f027052f23a 127 error_integral = error_integral + error * Ts;
ekiliptiay 0:8f027052f23a 128 double u_i = Ki * error_integral;
ekiliptiay 0:8f027052f23a 129
ekiliptiay 0:8f027052f23a 130 // Derivative part
ekiliptiay 0:8f027052f23a 131 double Kd = 10.1;
ekiliptiay 0:8f027052f23a 132 double error_derivative = (error - error_prev)/Ts;
ekiliptiay 0:8f027052f23a 133 double filtered_error_derivative = LowPassFilter.step(error_derivative);
ekiliptiay 0:8f027052f23a 134 double u_d = Kd * filtered_error_derivative;
ekiliptiay 0:8f027052f23a 135 error_prev = error;
ekiliptiay 0:8f027052f23a 136 // Sum all parts and return it
ekiliptiay 0:8f027052f23a 137 return u_k + u_i + u_d;
ekiliptiay 0:8f027052f23a 138 }
ekiliptiay 0:8f027052f23a 139
ekiliptiay 0:8f027052f23a 140 // PID controller for motor 2
ekiliptiay 0:8f027052f23a 141 double PID_controller2(double error){
ekiliptiay 0:8f027052f23a 142 static double error_integral = 0;
ekiliptiay 0:8f027052f23a 143 static double error_prev = error; // initialization with this value only done once!
ekiliptiay 0:8f027052f23a 144 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
ekiliptiay 0:8f027052f23a 145 double Ts = 0.01;
ekiliptiay 0:8f027052f23a 146
ekiliptiay 0:8f027052f23a 147 // Proportional part
ekiliptiay 0:8f027052f23a 148
ekiliptiay 0:8f027052f23a 149 double Kp = 0.1;
ekiliptiay 0:8f027052f23a 150 double u_k = Kp*error;
ekiliptiay 0:8f027052f23a 151
ekiliptiay 0:8f027052f23a 152 // Integral part
ekiliptiay 0:8f027052f23a 153 double Ki = 0.1;
ekiliptiay 0:8f027052f23a 154 error_integral = error_integral + error * Ts;
ekiliptiay 0:8f027052f23a 155 double u_i = Ki * error_integral;
ekiliptiay 0:8f027052f23a 156
ekiliptiay 0:8f027052f23a 157 // Derivative part
ekiliptiay 0:8f027052f23a 158 double Kd = 10.1;
ekiliptiay 0:8f027052f23a 159 double error_derivative = (error - error_prev)/Ts;
ekiliptiay 0:8f027052f23a 160 double filtered_error_derivative = LowPassFilter.step(error_derivative);
ekiliptiay 0:8f027052f23a 161 double u_d = Kd * filtered_error_derivative;
ekiliptiay 0:8f027052f23a 162 error_prev = error;
ekiliptiay 0:8f027052f23a 163 // Sum all parts and return it
ekiliptiay 0:8f027052f23a 164 return u_k + u_i + u_d;
ekiliptiay 0:8f027052f23a 165 }
ekiliptiay 0:8f027052f23a 166
ekiliptiay 0:8f027052f23a 167 void SetMotor1(double motorValue){
ekiliptiay 0:8f027052f23a 168 // Given -1<=motorValue<=1, this sets the PWM and direction
ekiliptiay 0:8f027052f23a 169 // bits for motor 1. Positive value makes motor rotating
ekiliptiay 0:8f027052f23a 170 // ccw. motorValues outside range are truncated to
ekiliptiay 0:8f027052f23a 171 // within range
ekiliptiay 0:8f027052f23a 172 if (motorValue >=0) directionpin1=1;
ekiliptiay 0:8f027052f23a 173 else directionpin1=0;
ekiliptiay 0:8f027052f23a 174 if (fabs(motorValue)>1) pwmpin1 = 1;
ekiliptiay 0:8f027052f23a 175 else pwmpin1 = fabs(motorValue);
ekiliptiay 0:8f027052f23a 176 }
ekiliptiay 0:8f027052f23a 177
ekiliptiay 0:8f027052f23a 178 void SetMotor2(double motorValue){
ekiliptiay 0:8f027052f23a 179 // Given -1<=motorValue<=1, this sets the PWM and direction
ekiliptiay 0:8f027052f23a 180 // bits for motor 1. Positive value makes motor rotating
ekiliptiay 0:8f027052f23a 181 // ccw. motorValues outside range are truncated to
ekiliptiay 0:8f027052f23a 182 // within range
ekiliptiay 0:8f027052f23a 183 if (motorValue >=0) directionpin2=1;
ekiliptiay 0:8f027052f23a 184 else directionpin2=0;
ekiliptiay 0:8f027052f23a 185 if (fabs(motorValue)>1) pwmpin2 = 1;
ekiliptiay 0:8f027052f23a 186 else pwmpin2 = fabs(motorValue);
ekiliptiay 0:8f027052f23a 187 }
ekiliptiay 0:8f027052f23a 188
ekiliptiay 0:8f027052f23a 189 int main()
ekiliptiay 0:8f027052f23a 190 {
ekiliptiay 0:8f027052f23a 191 while (true) {
ekiliptiay 0:8f027052f23a 192
ekiliptiay 0:8f027052f23a 193 }
ekiliptiay 0:8f027052f23a 194 }