Biorobotics_group_2 / Mbed 2 deprecated CompleteFunction

Alle subfiles samengevoegd

Dependencies:   HIDScope biquadFilter mbed MODSERIAL FastPWM QEI

main.cpp@20:04b5619a4fb7, 2016-11-02 (annotated)

Committer:
sjoerdbarts
Date:
Wed Nov 02 11:24:07 2016 +0000
Revision:
20:04b5619a4fb7
Parent:
19:3af738c38db0 
Final code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
s1600907 0:c96cf9760185 1 #include "mbed.h"
s1600907 0:c96cf9760185 2 #include "math.h"
s1600907 1:e1267e72ade8 3 #include "MODSERIAL.h"
sjoerdbarts 6:00011220b82b 4 #include "HIDScope.h"
sjoerdbarts 6:00011220b82b 5 #include "FastPWM.h"
sjoerdbarts 6:00011220b82b 6 #include "QEI.h"
sjoerdbarts 6:00011220b82b 7 #include "BiQuad.h"
sjoerdbarts 6:00011220b82b 8 #define SERIAL_BAUD 115200 // baud rate for serial communication
s1600907 0:c96cf9760185 9
sjoerdbarts 6:00011220b82b 10 // Serial connection
s1600907 1:e1267e72ade8 11 MODSERIAL pc(USBTX,USBRX);
sjoerdbarts 6:00011220b82b 12
s1600907 0:c96cf9760185 13 //EMG aansluitingen
s1600907 0:c96cf9760185 14 AnalogIn emg0( A0 ); //Biceps Rechts
s1600907 2:92593c9d6146 15 AnalogIn emg1( A1 ); //Bicpes Links
s1600907 2:92593c9d6146 16 AnalogIn emg2( A2 ); //Upper leg
sjoerdbarts 6:00011220b82b 17
sjoerdbarts 12:9b0451105991 18 // Setup Potmeters
sjoerdbarts 6:00011220b82b 19 // Potmeter 1 gives the reference position x
sjoerdbarts 6:00011220b82b 20 AnalogIn pot1(A3);
sjoerdbarts 6:00011220b82b 21 // Potmeter 2 gives the reference position y
sjoerdbarts 6:00011220b82b 22 AnalogIn pot2(A4);
sjoerdbarts 6:00011220b82b 23
sjoerdbarts 6:00011220b82b 24 // Setup Buttons
sjoerdbarts 6:00011220b82b 25 InterruptIn button1(PTB9); // button 1
sjoerdbarts 6:00011220b82b 26 InterruptIn button2(PTA1); // button 2
sjoerdbarts 6:00011220b82b 27 InterruptIn button3(PTC6); // SW2
sjoerdbarts 6:00011220b82b 28 InterruptIn button4(PTA4); // SW3
sjoerdbarts 6:00011220b82b 29
sjoerdbarts 8:676646c50aeb 30 // Setup botton values
sjoerdbarts 8:676646c50aeb 31 bool button1_value = false;
sjoerdbarts 8:676646c50aeb 32 bool button2_value = false;
sjoerdbarts 8:676646c50aeb 33
sjoerdbarts 6:00011220b82b 34 // Set motor Pinouts
sjoerdbarts 6:00011220b82b 35 DigitalOut motor1_dir(D4);
sjoerdbarts 6:00011220b82b 36 FastPWM motor1_pwm(D5);
sjoerdbarts 6:00011220b82b 37 DigitalOut motor2_dir(D7);
sjoerdbarts 6:00011220b82b 38 FastPWM motor2_pwm(D6);
sjoerdbarts 6:00011220b82b 39
sjoerdbarts 6:00011220b82b 40 // Set LED pins
sjoerdbarts 13:e8b8b035abbd 41 DigitalOut led(PTE24); // D15 LED1
sjoerdbarts 12:9b0451105991 42 DigitalOut red(LED_RED);
sjoerdbarts 12:9b0451105991 43 DigitalOut blue(LED_BLUE);
sjoerdbarts 12:9b0451105991 44 DigitalOut green(LED_GREEN);
sjoerdbarts 6:00011220b82b 45
sjoerdbarts 6:00011220b82b 46 // Set HID scope
sjoerdbarts 6:00011220b82b 47 HIDScope scope(6);
sjoerdbarts 6:00011220b82b 48
sjoerdbarts 6:00011220b82b 49 // Set encoder
sjoerdbarts 6:00011220b82b 50 QEI m1_EncoderCW(D10,D11,NC,32);
sjoerdbarts 6:00011220b82b 51 QEI m1_EncoderCCW(D11,D10,NC,32);
sjoerdbarts 6:00011220b82b 52 QEI m2_EncoderCW(D13,D12,NC,32);
sjoerdbarts 6:00011220b82b 53 QEI m2_EncoderCCW(D12,D13,NC,32);
s1600907 0:c96cf9760185 54
sjoerdbarts 7:bafc32b576c4 55 // Constants for control
sjoerdbarts 16:f9f2a0365224 56 volatile const int COUNTS_PER_REV = 4200;
sjoerdbarts 17:ea90b7340cbb 57 volatile const double DEGREES_PER_PULSE = 8400 / 360;
sjoerdbarts 7:bafc32b576c4 58
sjoerdbarts 7:bafc32b576c4 59 // Set initial Kp and Ki
sjoerdbarts 19:3af738c38db0 60 volatile double Kp = 0.05; // last known good Kp: 0.02
sjoerdbarts 20:04b5619a4fb7 61 volatile double Ki = 0.025; // last known good Ki: 0.0025
sjoerdbarts 7:bafc32b576c4 62 volatile double Kd = 0.0; // last known good Kd: 0.0
sjoerdbarts 7:bafc32b576c4 63
sjoerdbarts 8:676646c50aeb 64 // Set frequencies for sampling, calc and control.
sjoerdbarts 8:676646c50aeb 65 volatile const double SAMPLE_F = 500; // 500 Hz
sjoerdbarts 8:676646c50aeb 66 volatile const double SAMPLE_TS = 1.0/SAMPLE_F;
sjoerdbarts 12:9b0451105991 67 volatile const double CALC_F = 500.0; // 100 Hz
sjoerdbarts 8:676646c50aeb 68 volatile const double CALC_TS = 1.0/CALC_F;
sjoerdbarts 8:676646c50aeb 69 volatile const double CONTROLLER_TS = 0.01; // 100 Hz
sjoerdbarts 8:676646c50aeb 70
sjoerdbarts 7:bafc32b576c4 71 volatile double Ts = 0.01;
sjoerdbarts 7:bafc32b576c4 72 volatile double N = 100;
sjoerdbarts 7:bafc32b576c4 73
sjoerdbarts 7:bafc32b576c4 74 // Memory values for controllers
sjoerdbarts 7:bafc32b576c4 75 double m1_v1 = 0, m1_v2 = 0;
sjoerdbarts 7:bafc32b576c4 76 double m2_v1 = 0, m2_v2 = 0;
s1600907 0:c96cf9760185 77
s1600907 0:c96cf9760185 78 //BiQuad
s1600907 0:c96cf9760185 79 // making the biquads and chains; imported from matlab
s1600907 0:c96cf9760185 80 BiQuadChain bqc; //Chain for right biceps
s1600907 0:c96cf9760185 81 BiQuadChain bqc2; //Chain for left biceps
s1600907 0:c96cf9760185 82 BiQuadChain bqc3; //Chain for Upper leg
s1600907 0:c96cf9760185 83 // 1 to 3 are for right biceps
s1600907 0:c96cf9760185 84 BiQuad bq1( 9.14969e-01, -1.82994e+00, 9.14969e-01, -1.82269e+00, 8.37182e-01 );
s1600907 0:c96cf9760185 85 BiQuad bq2( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 );
s1600907 0:c96cf9760185 86 BiQuad bq3( 8.76555e-05, 1.75311e-04, 8.76555e-05, -1.97334e+00, 9.73695e-01 );
s1600907 0:c96cf9760185 87 // 4 to 6 are for left biceps
s1600907 0:c96cf9760185 88 BiQuad bq4( 9.14969e-01, -1.82994e+00, 9.14969e-01, -1.82269e+00, 8.37182e-01 );
s1600907 0:c96cf9760185 89 BiQuad bq5( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 );
s1600907 0:c96cf9760185 90 BiQuad bq6( 8.76555e-05, 1.75311e-04, 8.76555e-05, -1.97334e+00, 9.73695e-01 );
s1600907 0:c96cf9760185 91 // 7 to 9 are for upper leg
s1600907 0:c96cf9760185 92 BiQuad bq7( 9.14969e-01, -1.82994e+00, 9.14969e-01, -1.82269e+00, 8.37182e-01 );
s1600907 0:c96cf9760185 93 BiQuad bq8( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 );
s1600907 0:c96cf9760185 94 BiQuad bq9( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 );
s1600907 0:c96cf9760185 95
sjoerdbarts 7:bafc32b576c4 96 // Variables for filter
sjoerdbarts 7:bafc32b576c4 97 // muscle EMG < treshhold
s1600907 0:c96cf9760185 98 volatile bool BicepsLeft = false;
s1600907 0:c96cf9760185 99 volatile bool BicepsRight = false;
s1600907 0:c96cf9760185 100 volatile bool Leg = false;
s1600907 0:c96cf9760185 101
sjoerdbarts 7:bafc32b576c4 102 // Variables for MotorHoekBerekenen (MotorAngleCalculation)
s1600907 1:e1267e72ade8 103 volatile double x = 0.0; // beginpositie x en y
s1600907 1:e1267e72ade8 104 volatile double y = 305.5;
s1600907 0:c96cf9760185 105 volatile const double pi = 3.14159265359;
sjoerdbarts 19:3af738c38db0 106 volatile double Speed = 60; // Speed with which x and y are changed, in mm/s
sjoerdbarts 10:4bd8ec9e79ff 107 volatile double Theta1Gear = 60.0; // Beginpositie op 60 graden
s1600907 3:c524afedf863 108 volatile double Theta2Gear = 60.0;
sjoerdbarts 17:ea90b7340cbb 109 volatile const int LargeGear = 42;
sjoerdbarts 17:ea90b7340cbb 110 volatile const int SmallGear = 15;
sjoerdbarts 17:ea90b7340cbb 111 volatile double Theta1 = Theta1Gear*LargeGear/SmallGear; // Beginpositie van MotorHoek
sjoerdbarts 17:ea90b7340cbb 112 volatile double Theta2 = Theta2Gear*LargeGear/SmallGear;
sjoerdbarts 8:676646c50aeb 113
sjoerdbarts 10:4bd8ec9e79ff 114 // Stepper motor setup
sjoerdbarts 10:4bd8ec9e79ff 115 // Set constans
sjoerdbarts 10:4bd8ec9e79ff 116 volatile int stepmode = 16; // step mode
sjoerdbarts 10:4bd8ec9e79ff 117 volatile int steps = 150*stepmode; // amount of steps for 90 degree rotation = 50, full step mode: 1.8 degree per step.
sjoerdbarts 10:4bd8ec9e79ff 118 volatile int i = 0;
sjoerdbarts 10:4bd8ec9e79ff 119 volatile bool Stepper_CCW = false;
sjoerdbarts 10:4bd8ec9e79ff 120
sjoerdbarts 11:417193f23342 121 // Setup stepper motor pins
sjoerdbarts 12:9b0451105991 122 // DigitalOut pinMode(PTD2); //D11 NOW USES 3.3V OUPUT, SINCE THIS IS ALWAYS ON
sjoerdbarts 13:e8b8b035abbd 123 DigitalOut pinSleep(PTE25); //D14
sjoerdbarts 11:417193f23342 124 DigitalOut pinStep(PTC4); //D9
sjoerdbarts 11:417193f23342 125 DigitalOut pinDir(PTC12); //D8
sjoerdbarts 11:417193f23342 126
sjoerdbarts 11:417193f23342 127 // Control of the stepper motor with 1 Ticker and 1 Timeout
sjoerdbarts 11:417193f23342 128 Ticker TickerStepper;
sjoerdbarts 11:417193f23342 129 Timeout TimeoutStepper;
sjoerdbarts 11:417193f23342 130
sjoerdbarts 10:4bd8ec9e79ff 131 // Global variable Step_State
s1600907 0:c96cf9760185 132 volatile bool Stepper_State = false; // false = we zijn niet bezig met flippen
sjoerdbarts 10:4bd8ec9e79ff 133
sjoerdbarts 13:e8b8b035abbd 134 // Setup Tickers
sjoerdbarts 13:e8b8b035abbd 135 Ticker CalculationsTicker;
sjoerdbarts 13:e8b8b035abbd 136 Ticker BlinkLedTicker;
sjoerdbarts 13:e8b8b035abbd 137 Ticker PIDControlTicker;
sjoerdbarts 13:e8b8b035abbd 138 Ticker PotControlTicker;
sjoerdbarts 13:e8b8b035abbd 139
sjoerdbarts 10:4bd8ec9e79ff 140 /////////////////////// Functtions for stepper motor /////////////////////////////////////
sjoerdbarts 10:4bd8ec9e79ff 141 // Set the Step pin to 0
sjoerdbarts 10:4bd8ec9e79ff 142 void StepperFall()
sjoerdbarts 10:4bd8ec9e79ff 143 {
sjoerdbarts 10:4bd8ec9e79ff 144 pinStep = 0;
sjoerdbarts 10:4bd8ec9e79ff 145 }
sjoerdbarts 10:4bd8ec9e79ff 146 // Turn off stepper and power off driver board. Reset Stepper_CCS, i and Stepper_State
sjoerdbarts 10:4bd8ec9e79ff 147 void Stepper_Off()
sjoerdbarts 10:4bd8ec9e79ff 148 {
sjoerdbarts 10:4bd8ec9e79ff 149 TickerStepper.detach();
sjoerdbarts 10:4bd8ec9e79ff 150 pinSleep = 0;
sjoerdbarts 10:4bd8ec9e79ff 151 i=0;
sjoerdbarts 10:4bd8ec9e79ff 152 Stepper_State = false;
sjoerdbarts 10:4bd8ec9e79ff 153 Stepper_CCW = false;
sjoerdbarts 10:4bd8ec9e79ff 154 }
sjoerdbarts 10:4bd8ec9e79ff 155
sjoerdbarts 10:4bd8ec9e79ff 156 // Change direction
sjoerdbarts 10:4bd8ec9e79ff 157 void Stepper_Change_Dir()
sjoerdbarts 10:4bd8ec9e79ff 158 {
sjoerdbarts 10:4bd8ec9e79ff 159 pinDir = true;
sjoerdbarts 10:4bd8ec9e79ff 160 Stepper_CCW = true;
sjoerdbarts 10:4bd8ec9e79ff 161 i = 0;
sjoerdbarts 10:4bd8ec9e79ff 162 }
sjoerdbarts 10:4bd8ec9e79ff 163
sjoerdbarts 10:4bd8ec9e79ff 164 // Move the motor 1 step
sjoerdbarts 10:4bd8ec9e79ff 165 void StepperRise()
sjoerdbarts 10:4bd8ec9e79ff 166 {
sjoerdbarts 10:4bd8ec9e79ff 167 if((i<steps)&&(not Stepper_CCW))
sjoerdbarts 10:4bd8ec9e79ff 168 {
sjoerdbarts 10:4bd8ec9e79ff 169 pinStep=1; // Set stepper to 1
sjoerdbarts 10:4bd8ec9e79ff 170 // attach timeout to set it back to 0, min step duration is 1.8 us
sjoerdbarts 10:4bd8ec9e79ff 171 TimeoutStepper.attach_us(&StepperFall, 2);
sjoerdbarts 10:4bd8ec9e79ff 172 i=i+1;
sjoerdbarts 10:4bd8ec9e79ff 173 }
sjoerdbarts 10:4bd8ec9e79ff 174 else if (Stepper_CCW)
sjoerdbarts 10:4bd8ec9e79ff 175 {
sjoerdbarts 10:4bd8ec9e79ff 176 if(i<steps)
sjoerdbarts 10:4bd8ec9e79ff 177 {
sjoerdbarts 10:4bd8ec9e79ff 178 pinStep=1;
sjoerdbarts 10:4bd8ec9e79ff 179 TimeoutStepper.attach_us(&StepperFall, 2);
sjoerdbarts 10:4bd8ec9e79ff 180 i=i+1;
sjoerdbarts 10:4bd8ec9e79ff 181 }
sjoerdbarts 10:4bd8ec9e79ff 182 else
sjoerdbarts 10:4bd8ec9e79ff 183 {
sjoerdbarts 10:4bd8ec9e79ff 184 Stepper_Off();
sjoerdbarts 10:4bd8ec9e79ff 185 }
sjoerdbarts 10:4bd8ec9e79ff 186 }
sjoerdbarts 10:4bd8ec9e79ff 187 else
sjoerdbarts 10:4bd8ec9e79ff 188 {
sjoerdbarts 10:4bd8ec9e79ff 189 Stepper_Change_Dir();
sjoerdbarts 10:4bd8ec9e79ff 190 }
sjoerdbarts 10:4bd8ec9e79ff 191 }
sjoerdbarts 10:4bd8ec9e79ff 192
sjoerdbarts 10:4bd8ec9e79ff 193 // Activate the stepper motor
sjoerdbarts 10:4bd8ec9e79ff 194 // at a constant speed
sjoerdbarts 10:4bd8ec9e79ff 195 void Stepper_On()
sjoerdbarts 10:4bd8ec9e79ff 196 {
sjoerdbarts 13:e8b8b035abbd 197 if (Stepper_State == false)
sjoerdbarts 13:e8b8b035abbd 198 {
sjoerdbarts 10:4bd8ec9e79ff 199 // Set global variable to true
sjoerdbarts 10:4bd8ec9e79ff 200 Stepper_State = true;
sjoerdbarts 10:4bd8ec9e79ff 201 // Power on the board and set direction to CW
sjoerdbarts 10:4bd8ec9e79ff 202 pinSleep = 1;
sjoerdbarts 10:4bd8ec9e79ff 203 pinDir = false;
sjoerdbarts 10:4bd8ec9e79ff 204 // Calc the speed of the stepper
sjoerdbarts 10:4bd8ec9e79ff 205 int nPPS = 50*stepmode; // was 1200, this controlls the speed, keep this times the step mode
sjoerdbarts 11:417193f23342 206 double fFrequency_Hz = 1.f * nPPS;
sjoerdbarts 11:417193f23342 207 double fPeriod_s = 1.0f / fFrequency_Hz;
sjoerdbarts 10:4bd8ec9e79ff 208 // Check if the speed is too fast small and report back if that is the case
sjoerdbarts 10:4bd8ec9e79ff 209 if (fPeriod_s < 2e-6)
sjoerdbarts 10:4bd8ec9e79ff 210 {
sjoerdbarts 10:4bd8ec9e79ff 211 pc.printf("\r\n ERROR: fPeriod_s too small \r\n");
sjoerdbarts 10:4bd8ec9e79ff 212 fPeriod_s=2e-6;
sjoerdbarts 10:4bd8ec9e79ff 213 }
sjoerdbarts 10:4bd8ec9e79ff 214
sjoerdbarts 10:4bd8ec9e79ff 215 TickerStepper.attach(&StepperRise, // void(*fptr)(void)
sjoerdbarts 10:4bd8ec9e79ff 216 fPeriod_s // float t
sjoerdbarts 10:4bd8ec9e79ff 217 );
sjoerdbarts 13:e8b8b035abbd 218 }
sjoerdbarts 10:4bd8ec9e79ff 219 }
s1600907 0:c96cf9760185 220
sjoerdbarts 7:bafc32b576c4 221 /////////////////////// functions for filters /////////////////////////////////////////////
s1600907 0:c96cf9760185 222 void sample()
s1600907 0:c96cf9760185 223 {
s1600907 0:c96cf9760185 224 /* Sample function
s1600907 0:c96cf9760185 225 samples the emg0 (of the rightBiceps)
s1600907 0:c96cf9760185 226 samples the emg1 (of the leftBiceps)
s1600907 0:c96cf9760185 227 samples the emg2 (of the Upper leg)
s1600907 0:c96cf9760185 228 filter the singals
s1600907 0:c96cf9760185 229 sends it to HIDScope
s1600907 0:c96cf9760185 230 */
s1600907 0:c96cf9760185 231
s1600907 0:c96cf9760185 232 // Rechts Biceps
s1600907 0:c96cf9760185 233 double inRechts = emg0.read(); // EMG signal
s1600907 0:c96cf9760185 234 double FilterRechts = bqc.step(inRechts); // High pass + Notch (50 HZ)
s1600907 0:c96cf9760185 235 double RectifyRechts = fabs(FilterRechts); // Rectify
s1600907 0:c96cf9760185 236 double outRechts = bq3.step(RectifyRechts); // Low pass
s1600907 2:92593c9d6146 237
s1600907 0:c96cf9760185 238 // Links Biceps
s1600907 0:c96cf9760185 239 double inLinks = emg1.read(); // EMG signal
s1600907 0:c96cf9760185 240 double FilterLinks = bqc2.step(inLinks); // High pass + Notch (50 HZ)
s1600907 0:c96cf9760185 241 double RectifyLinks = fabs(FilterLinks); // Rectify
s1600907 0:c96cf9760185 242 double outLinks = bq6.step(RectifyLinks); // Low pass
s1600907 0:c96cf9760185 243
s1600907 0:c96cf9760185 244 // Upper leg
s1600907 0:c96cf9760185 245 double inBeen = emg2.read(); // EMG signal
s1600907 0:c96cf9760185 246 double FilterBeen = bqc3.step(inBeen); // High pass + Notch (50 HZ)
s1600907 0:c96cf9760185 247 double RectifyBeen = fabs(FilterBeen); // Rectify
s1600907 2:92593c9d6146 248 double outBeen = bq9.step(RectifyBeen); // Low pass
s1600907 0:c96cf9760185 249
s1600907 0:c96cf9760185 250
s1600907 0:c96cf9760185 251 /* EMG signal in channel 0 (the first channel)
s1600907 0:c96cf9760185 252 and the filtered EMG signal in channel 1 (the second channel)
s1600907 0:c96cf9760185 253 of the HIDscope */
s1600907 5:b4a0301ec8cc 254 /*scope.set(0,inRechts);
s1600907 0:c96cf9760185 255 scope.set(1,outRechts);
s1600907 2:92593c9d6146 256 scope.set(2,inLinks);
s1600907 0:c96cf9760185 257 scope.set(3,outLinks);
s1600907 0:c96cf9760185 258 scope.set(4,inBeen);
s1600907 0:c96cf9760185 259 scope.set(5,outBeen);
s1600907 5:b4a0301ec8cc 260 */
s1600907 0:c96cf9760185 261 // To indicate that the function is working, the LED is on
sjoerdbarts 15:e9439eac1342 262 if (outRechts >= 0.03){
s1600907 0:c96cf9760185 263 BicepsRight = true;
sjoerdbarts 14:f114e57c3795 264 red = 0;
s1600907 0:c96cf9760185 265 }
s1600907 0:c96cf9760185 266 else{
s1600907 0:c96cf9760185 267 BicepsRight = false;
sjoerdbarts 14:f114e57c3795 268 red = 1;
s1600907 0:c96cf9760185 269 }
s1600907 2:92593c9d6146 270
s1600907 0:c96cf9760185 271 // If Biceps links is actuated then:
sjoerdbarts 18:32e9db0d47c9 272 if (outLinks >= 0.02){
s1600907 0:c96cf9760185 273 BicepsLeft = true;
sjoerdbarts 18:32e9db0d47c9 274 green = 0;
s1600907 0:c96cf9760185 275 }
s1600907 0:c96cf9760185 276 else{
s1600907 0:c96cf9760185 277 BicepsLeft = false;
sjoerdbarts 18:32e9db0d47c9 278 green = 1;
s1600907 0:c96cf9760185 279 }
s1600907 0:c96cf9760185 280 // If upper leg is actuated then:
sjoerdbarts 15:e9439eac1342 281 if (outBeen >= 0.02){
s1600907 0:c96cf9760185 282 Leg = true;
sjoerdbarts 18:32e9db0d47c9 283 blue = 0;
s1600907 0:c96cf9760185 284 }
s1600907 0:c96cf9760185 285 else{
s1600907 0:c96cf9760185 286 Leg = false;
sjoerdbarts 18:32e9db0d47c9 287 blue = 1;
s1600907 0:c96cf9760185 288 }
s1600907 0:c96cf9760185 289 }
s1600907 0:c96cf9760185 290
sjoerdbarts 7:bafc32b576c4 291 ///////////////////// functions for "motorhoekberekenen" //////////////////////////
sjoerdbarts 7:bafc32b576c4 292 // save previous x
s1600907 0:c96cf9760185 293 double Calc_Prev_x () {
s1600907 0:c96cf9760185 294 double Prev_x = x;
s1600907 0:c96cf9760185 295 //pc.printf("prev x = %f\r\n", Prev_x);
s1600907 0:c96cf9760185 296 return Prev_x;
s1600907 0:c96cf9760185 297 }
s1600907 0:c96cf9760185 298
sjoerdbarts 7:bafc32b576c4 299 // save previous y
s1600907 0:c96cf9760185 300 double Calc_Prev_y () {
s1600907 0:c96cf9760185 301 double Prev_y = y;
s1600907 0:c96cf9760185 302 //pc.printf("prev y = %f\r\n", Prev_y);
s1600907 0:c96cf9760185 303 return Prev_y;
s1600907 0:c96cf9760185 304 }
s1600907 0:c96cf9760185 305
sjoerdbarts 7:bafc32b576c4 306 // save previous Theta1Gear
s1600907 4:05c5da1c8b08 307 double Calc_Prev_Theta1_Gear () {
s1600907 4:05c5da1c8b08 308 double Prev_Theta1_Gear = Theta1Gear;
s1600907 4:05c5da1c8b08 309 return Prev_Theta1_Gear;
s1600907 4:05c5da1c8b08 310 }
s1600907 4:05c5da1c8b08 311
sjoerdbarts 7:bafc32b576c4 312 // save previous Theta2Gear
s1600907 4:05c5da1c8b08 313 double Calc_Prev_Theta2_Gear () {
s1600907 4:05c5da1c8b08 314 double Prev_Theta2_Gear = Theta2Gear;
s1600907 4:05c5da1c8b08 315 return Prev_Theta2_Gear;
s1600907 4:05c5da1c8b08 316 }
s1600907 4:05c5da1c8b08 317
sjoerdbarts 8:676646c50aeb 318 void CalcXY ()
sjoerdbarts 7:bafc32b576c4 319 {
sjoerdbarts 8:676646c50aeb 320 // calc steps in mm
sjoerdbarts 8:676646c50aeb 321 double Step = Speed/CALC_F ; //10 mm per seconde afleggen
sjoerdbarts 20:04b5619a4fb7 322 int Ymax = 306;
sjoerdbarts 20:04b5619a4fb7 323 int Xmax = 200;
s1600907 0:c96cf9760185 324 if (BicepsLeft==true && BicepsRight==true && Leg==true && Stepper_State==false) {
sjoerdbarts 11:417193f23342 325 Stepper_On();
s1600907 0:c96cf9760185 326 }
s1600907 0:c96cf9760185 327 else if (BicepsLeft==true && BicepsRight==false && Leg==false && Stepper_State==false) {
s1600907 0:c96cf9760185 328 x = x - Step;
s1600907 0:c96cf9760185 329 }
s1600907 0:c96cf9760185 330 else if (BicepsLeft==false && BicepsRight==true && Leg==false && Stepper_State==false) {
s1600907 0:c96cf9760185 331 x = x + Step; // naar Right bewegen
s1600907 0:c96cf9760185 332 }
s1600907 0:c96cf9760185 333 else if (BicepsLeft==true && BicepsRight==true && Leg==false && Stepper_State==false) {
s1600907 0:c96cf9760185 334 y = y + Step; // naar voren bewegen
s1600907 0:c96cf9760185 335 }
s1600907 0:c96cf9760185 336 else if (BicepsLeft==false && BicepsRight==true && Leg==true && Stepper_State==false) {
s1600907 0:c96cf9760185 337 y = y - Step; // naar achter bewegen
s1600907 0:c96cf9760185 338 }
s1600907 0:c96cf9760185 339 else if (BicepsLeft==false && BicepsRight==false && Leg==false || Stepper_State==true) {
s1600907 0:c96cf9760185 340 }
s1600907 0:c96cf9760185 341
s1600907 0:c96cf9760185 342 // Grenswaardes LET OP: ARMEN MISSCHIEN GEBLOKKEERD DOOR BALK AAN DE BINNENKANT
sjoerdbarts 20:04b5619a4fb7 343 if (x > Xmax) {
sjoerdbarts 20:04b5619a4fb7 344 x = Xmax;
s1600907 0:c96cf9760185 345 }
sjoerdbarts 20:04b5619a4fb7 346 else if (x < -Xmax) {
sjoerdbarts 20:04b5619a4fb7 347 x = -Xmax;
s1600907 0:c96cf9760185 348 }
sjoerdbarts 20:04b5619a4fb7 349 if (y > Ymax) {
sjoerdbarts 20:04b5619a4fb7 350 y = Ymax;
s1600907 0:c96cf9760185 351 }
s1600907 0:c96cf9760185 352 else if (y < 50) {
s1600907 0:c96cf9760185 353 y = 50; // GOKJE, UITPROBEREN
s1600907 0:c96cf9760185 354 }
s1600907 5:b4a0301ec8cc 355 //pc.printf("x = %f, y = %f\r\n", x, y);
s1600907 1:e1267e72ade8 356
s1600907 2:92593c9d6146 357 //scope.set(2,x);
s1600907 2:92593c9d6146 358 //scope.set(3,y);
s1600907 0:c96cf9760185 359 }
s1600907 0:c96cf9760185 360
s1600907 0:c96cf9760185 361 // diagonaal berekenen voor linker arm
s1600907 0:c96cf9760185 362 double CalcDia1 (double x, double y) {
s1600907 0:c96cf9760185 363 double a = 50.0; // de afstand van gekozen assenstelsel tot de armas (assenstelsel precies in het midden) KEERTJE NAMETEN
s1600907 0:c96cf9760185 364 double BV1 = sqrt(pow((a+x),2) + pow(y,2)); // diagonaal (afstand van armas tot locatie) berekenen
s1600907 0:c96cf9760185 365 double Dia1 = pow(BV1,2)/(2*BV1); // berekenen van de afstand oorsprong tot diagonaal
s1600907 0:c96cf9760185 366
s1600907 0:c96cf9760185 367 //pc.printf("dia = %f, x = %f, y= %f\r\n", Dia1, x, y);
s1600907 0:c96cf9760185 368 return Dia1;
s1600907 0:c96cf9760185 369 }
s1600907 0:c96cf9760185 370
s1600907 0:c96cf9760185 371 // diagonaal berekenen voor rechter arm
s1600907 0:c96cf9760185 372 double CalcDia2 (double x, double y) {
s1600907 0:c96cf9760185 373 double a = 50.0;
s1600907 0:c96cf9760185 374 double BV2 = sqrt(pow((x-a),2) + pow(y,2)); // zelfde nog een keer doen maar nu voor de rechter arm
s1600907 0:c96cf9760185 375 double Dia2 = pow(BV2,2)/(2*BV2);
s1600907 0:c96cf9760185 376
s1600907 0:c96cf9760185 377 //pc.printf("dia = %f, x = %f, y= %f\r\n", Dia2, x, y);
s1600907 0:c96cf9760185 378 return Dia2;
s1600907 0:c96cf9760185 379 }
s1600907 0:c96cf9760185 380
s1600907 0:c96cf9760185 381 // calculate Theta1
s1600907 4:05c5da1c8b08 382 void CalcTheta1 (double Dia1) {
s1600907 0:c96cf9760185 383 double a = 50.0;
s1600907 0:c96cf9760185 384 double Bar = 200.0; // lengte van de armen
s1600907 0:c96cf9760185 385
s1600907 0:c96cf9760185 386 // Hoek berekenen van het grote tandwiel (gear)
s1600907 0:c96cf9760185 387 if (x > -a) {
s1600907 0:c96cf9760185 388 Theta1Gear = pi - atan(y/(x+a)) - acos(Dia1/Bar);
s1600907 0:c96cf9760185 389 }
sjoerdbarts 19:3af738c38db0 390 else if (x < -a) {
s1600907 0:c96cf9760185 391 Theta1Gear = pi - (pi + atan(y/(x+a))) - acos(Dia1/Bar);
s1600907 0:c96cf9760185 392 }
s1600907 0:c96cf9760185 393 else { // als x=-a
s1600907 0:c96cf9760185 394 Theta1Gear = 0.5*pi - acos(Dia1/Bar);
s1600907 0:c96cf9760185 395 }
s1600907 3:c524afedf863 396 Theta1Gear = Theta1Gear*180.0/pi; // veranderen van radialen naar graden
s1600907 0:c96cf9760185 397
s1600907 0:c96cf9760185 398 // omrekenen van grote tandwiel naar motortandwiel
sjoerdbarts 17:ea90b7340cbb 399 Theta1 = Theta1Gear*LargeGear/SmallGear; // grote tandwiel heeft 42 tanden. Motortandwiel heeft er 12.
s1600907 0:c96cf9760185 400
sjoerdbarts 18:32e9db0d47c9 401 //pc.printf("thetaMotor = %f, thetaGear = %f\r\n", Theta1, Theta1Gear);
s1600907 0:c96cf9760185 402 }
s1600907 0:c96cf9760185 403
s1600907 4:05c5da1c8b08 404 void CalcTheta2 (double Dia2) {
s1600907 0:c96cf9760185 405 double a = 50.0;
s1600907 0:c96cf9760185 406 double Bar = 200.0; // lengte van de armen
s1600907 0:c96cf9760185 407 double Prev_Theta2_Gear = Theta2Gear;
s1600907 4:05c5da1c8b08 408
s1600907 0:c96cf9760185 409 // Hoek berekenen van het grote tandwiel (gear)
s1600907 0:c96cf9760185 410 if (x < a) {
s1600907 0:c96cf9760185 411 Theta2Gear = pi + atan(y/(x-a)) - acos(Dia2/Bar);
s1600907 0:c96cf9760185 412 }
s1600907 0:c96cf9760185 413 else if (x > a) {
s1600907 0:c96cf9760185 414 Theta2Gear = pi - (pi - atan(y/(x-a))) - acos(Dia2/Bar);
s1600907 0:c96cf9760185 415 }
s1600907 0:c96cf9760185 416 else { // als x=a
s1600907 0:c96cf9760185 417 Theta2Gear = 0.5*pi - acos(Dia2/Bar);
s1600907 0:c96cf9760185 418 }
s1600907 3:c524afedf863 419 Theta2Gear = Theta2Gear*180/pi; // veranderen van radialen naar graden
s1600907 0:c96cf9760185 420
s1600907 4:05c5da1c8b08 421 // omrekenen van grote tandwiel naar motortandwiel
sjoerdbarts 17:ea90b7340cbb 422 Theta2 = Theta2Gear*LargeGear/SmallGear; // grote tandwiel heeft 42 tanden. Motortandwiel heeft er 12.
s1600907 4:05c5da1c8b08 423
sjoerdbarts 18:32e9db0d47c9 424 //pc.printf("thetaMotor = %f, thetaGear = %f\r\n", Theta2, Theta2Gear);
s1600907 4:05c5da1c8b08 425 }
s1600907 4:05c5da1c8b08 426
s1600907 4:05c5da1c8b08 427 // als een van de hoeken te groot wordt, zet dan alles terug naar de vorige positie
s1600907 4:05c5da1c8b08 428 void AngleLimits (double Prev_Theta1_Gear, double Prev_Theta2_Gear, double Prev_x, double Prev_y) {
s1600907 4:05c5da1c8b08 429 double MaxThetaGear = 100.0; // de hoek voordat arm het opstakel raakt (max hoek is 107.62 tussen arm en opstakel, maken er 100 van voor veiligheid)
s1600907 4:05c5da1c8b08 430
s1600907 4:05c5da1c8b08 431 if (Theta1Gear >= MaxThetaGear || Theta2Gear >= MaxThetaGear) {
s1600907 4:05c5da1c8b08 432 Theta1Gear = Prev_Theta1_Gear;
s1600907 0:c96cf9760185 433 Theta2Gear = Prev_Theta2_Gear;
s1600907 0:c96cf9760185 434 x = Prev_x;
s1600907 0:c96cf9760185 435 y = Prev_y;
s1600907 4:05c5da1c8b08 436
sjoerdbarts 17:ea90b7340cbb 437 Theta1 = Theta1Gear*LargeGear/SmallGear;
sjoerdbarts 17:ea90b7340cbb 438 Theta2 = Theta2Gear*LargeGear/SmallGear;
s1600907 0:c96cf9760185 439 }
sjoerdbarts 18:32e9db0d47c9 440 //pc.printf("thetaMotor = %f, thetaGear = %f\r\n", Theta2, Theta2Gear);
s1600907 0:c96cf9760185 441 }
s1600907 0:c96cf9760185 442
s1600907 0:c96cf9760185 443 void CalculationsForTheta () {
sjoerdbarts 12:9b0451105991 444 sample();
s1600907 0:c96cf9760185 445 double Prev_x = Calc_Prev_x ();
s1600907 0:c96cf9760185 446 double Prev_y = Calc_Prev_y ();
sjoerdbarts 8:676646c50aeb 447 double Prev_Theta1_Gear = Calc_Prev_Theta1_Gear ();
sjoerdbarts 8:676646c50aeb 448 double Prev_Theta2_Gear = Calc_Prev_Theta2_Gear ();
sjoerdbarts 8:676646c50aeb 449 CalcXY();
s1600907 0:c96cf9760185 450 double Dia1 = CalcDia1 (x, y);
s1600907 0:c96cf9760185 451 double Dia2 = CalcDia2 (x, y);
s1600907 4:05c5da1c8b08 452 CalcTheta1 (Dia1);
sjoerdbarts 8:676646c50aeb 453 CalcTheta2 (Dia2);
sjoerdbarts 18:32e9db0d47c9 454 //AngleLimits (Prev_Theta1_Gear, Prev_Theta2_Gear, Prev_x, Prev_y); // laatste check
s1600907 0:c96cf9760185 455 }
s1600907 0:c96cf9760185 456
sjoerdbarts 8:676646c50aeb 457 ////////////////////////// Code for control /////////////////////////////////////
sjoerdbarts 8:676646c50aeb 458
sjoerdbarts 8:676646c50aeb 459 double m1_GetPosition()
sjoerdbarts 8:676646c50aeb 460 {
sjoerdbarts 8:676646c50aeb 461 int countsCW = m1_EncoderCW.getPulses();
sjoerdbarts 8:676646c50aeb 462 int countsCCW= m1_EncoderCCW.getPulses();
sjoerdbarts 8:676646c50aeb 463 int net_counts=countsCW-countsCCW;
sjoerdbarts 8:676646c50aeb 464 double Position=(net_counts*360.0)/COUNTS_PER_REV;
sjoerdbarts 8:676646c50aeb 465 return Position;
sjoerdbarts 8:676646c50aeb 466 }
sjoerdbarts 8:676646c50aeb 467
sjoerdbarts 8:676646c50aeb 468 double m2_GetPosition()
sjoerdbarts 8:676646c50aeb 469 {
sjoerdbarts 8:676646c50aeb 470 int countsCW = m2_EncoderCW.getPulses();
sjoerdbarts 8:676646c50aeb 471 int countsCCW= m2_EncoderCCW.getPulses();
sjoerdbarts 8:676646c50aeb 472 int net_counts=countsCW-countsCCW;
sjoerdbarts 8:676646c50aeb 473 double Position=(net_counts*360.0)/COUNTS_PER_REV;
sjoerdbarts 8:676646c50aeb 474 return Position;
sjoerdbarts 8:676646c50aeb 475 }
sjoerdbarts 8:676646c50aeb 476
sjoerdbarts 8:676646c50aeb 477 // Position control when calibrating
sjoerdbarts 8:676646c50aeb 478 double m1_GetPosition_cal()
sjoerdbarts 8:676646c50aeb 479 {
sjoerdbarts 8:676646c50aeb 480 int countsCW = m1_EncoderCW.getPulses();
sjoerdbarts 8:676646c50aeb 481 int countsCCW= m1_EncoderCCW.getPulses();
sjoerdbarts 8:676646c50aeb 482 int net_counts=countsCW-countsCCW;
sjoerdbarts 17:ea90b7340cbb 483 double Position=(net_counts*360.0)/COUNTS_PER_REV+60.0f*LargeGear/SmallGear; // calibrated position is 210 degrees
sjoerdbarts 8:676646c50aeb 484 return Position;
sjoerdbarts 8:676646c50aeb 485 }
sjoerdbarts 8:676646c50aeb 486
sjoerdbarts 8:676646c50aeb 487 // Position control when calibrating
sjoerdbarts 8:676646c50aeb 488 double m2_GetPosition_cal()
sjoerdbarts 8:676646c50aeb 489 {
sjoerdbarts 8:676646c50aeb 490 int countsCW = m2_EncoderCW.getPulses();
sjoerdbarts 8:676646c50aeb 491 int countsCCW= m2_EncoderCCW.getPulses();
sjoerdbarts 8:676646c50aeb 492 int net_counts=countsCW-countsCCW;
sjoerdbarts 17:ea90b7340cbb 493 double Position=(net_counts*360.0)/COUNTS_PER_REV+60.0f*LargeGear/SmallGear; // calibrated position is 210 degrees
sjoerdbarts 8:676646c50aeb 494 return Position;
sjoerdbarts 8:676646c50aeb 495 }
sjoerdbarts 8:676646c50aeb 496 double m1_PID(double error, const double Kp, const double Ki, const double Kd, const double Ts, const double N, double &m1_v1, double &m1_v2)
sjoerdbarts 8:676646c50aeb 497 {
sjoerdbarts 8:676646c50aeb 498 double a1 = -4/(N*Ts+2),
sjoerdbarts 8:676646c50aeb 499 a2 = -1*(N*Ts - 2)/(N*Ts+2),
sjoerdbarts 8:676646c50aeb 500 b0 = (4*Kp + 4*Kd*N + 2*Ki*Ts + 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4),
sjoerdbarts 8:676646c50aeb 501 b1 = (Ki*N*pow(Ts,2) - 4*Kp - 4*Kd*N)/(N*Ts + 2),
sjoerdbarts 8:676646c50aeb 502 b2 = (4*Kp + 4*Kd*N - 2*Ki*Ts - 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4);
sjoerdbarts 8:676646c50aeb 503
sjoerdbarts 8:676646c50aeb 504 double v = error - a1*m1_v1 - a2*m1_v2;
sjoerdbarts 8:676646c50aeb 505 double u = b0*v + b1*m1_v1 + b2*m1_v2;
sjoerdbarts 8:676646c50aeb 506 m1_v2 = m1_v1; m1_v1 = v;
sjoerdbarts 8:676646c50aeb 507 return u;
sjoerdbarts 8:676646c50aeb 508 }
sjoerdbarts 8:676646c50aeb 509
sjoerdbarts 8:676646c50aeb 510 double m2_PID(double error, const double Kp, const double Ki, const double Kd, const double Ts, const double N, double &m2_v1, double &m2_v2)
sjoerdbarts 8:676646c50aeb 511 {
sjoerdbarts 8:676646c50aeb 512 double a1 = -4/(N*Ts+2),
sjoerdbarts 8:676646c50aeb 513 a2 = -1*(N*Ts - 2)/(N*Ts+2),
sjoerdbarts 8:676646c50aeb 514 b0 = (4*Kp + 4*Kd*N + 2*Ki*Ts + 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4),
sjoerdbarts 8:676646c50aeb 515 b1 = (Ki*N*pow(Ts,2) - 4*Kp - 4*Kd*N)/(N*Ts + 2),
sjoerdbarts 8:676646c50aeb 516 b2 = (4*Kp + 4*Kd*N - 2*Ki*Ts - 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4);
sjoerdbarts 8:676646c50aeb 517
sjoerdbarts 8:676646c50aeb 518 double v = error - a1*m1_v1 - a2*m1_v2;
sjoerdbarts 8:676646c50aeb 519 double u = b0*v + b1*m1_v1 + b2*m1_v2;
sjoerdbarts 8:676646c50aeb 520 m1_v2 = m1_v1; m1_v1 = v;
sjoerdbarts 8:676646c50aeb 521 return u;
sjoerdbarts 8:676646c50aeb 522 }
sjoerdbarts 8:676646c50aeb 523
sjoerdbarts 8:676646c50aeb 524
s1600907 0:c96cf9760185 525
sjoerdbarts 8:676646c50aeb 526 void SetMotor(int motor_number, double MotorValue)
sjoerdbarts 8:676646c50aeb 527 {
sjoerdbarts 8:676646c50aeb 528 // Given -1<=motorValue<=1, this sets the PWM and direction
sjoerdbarts 8:676646c50aeb 529 // bits for motor 1. Positive value makes motor rotating
sjoerdbarts 8:676646c50aeb 530 // clockwise. motorValues outside range are truncated to
sjoerdbarts 8:676646c50aeb 531 // within range
sjoerdbarts 8:676646c50aeb 532 if (motor_number == 1)
sjoerdbarts 8:676646c50aeb 533 {
sjoerdbarts 8:676646c50aeb 534 if (MotorValue >=0)
sjoerdbarts 8:676646c50aeb 535 {
sjoerdbarts 8:676646c50aeb 536 motor1_dir=0;
sjoerdbarts 8:676646c50aeb 537 }
sjoerdbarts 8:676646c50aeb 538 else
sjoerdbarts 8:676646c50aeb 539 {
sjoerdbarts 8:676646c50aeb 540 motor1_dir=1;
sjoerdbarts 8:676646c50aeb 541 }
sjoerdbarts 8:676646c50aeb 542 if (fabs(MotorValue)>1){
sjoerdbarts 8:676646c50aeb 543 motor1_pwm.write(1);
sjoerdbarts 8:676646c50aeb 544 }
sjoerdbarts 8:676646c50aeb 545 else
sjoerdbarts 8:676646c50aeb 546 {
sjoerdbarts 8:676646c50aeb 547 motor1_pwm.write(abs(MotorValue));
sjoerdbarts 8:676646c50aeb 548 }
sjoerdbarts 8:676646c50aeb 549 }
sjoerdbarts 8:676646c50aeb 550 else
sjoerdbarts 8:676646c50aeb 551 {
sjoerdbarts 8:676646c50aeb 552 if (MotorValue >=0)
sjoerdbarts 8:676646c50aeb 553 {
sjoerdbarts 8:676646c50aeb 554 motor2_dir=0;
sjoerdbarts 8:676646c50aeb 555 }
sjoerdbarts 8:676646c50aeb 556 else
sjoerdbarts 8:676646c50aeb 557 {
sjoerdbarts 8:676646c50aeb 558 motor2_dir=1;
sjoerdbarts 8:676646c50aeb 559 }
sjoerdbarts 8:676646c50aeb 560 if (fabs(MotorValue)>1){
sjoerdbarts 8:676646c50aeb 561 motor2_pwm.write(1);
sjoerdbarts 8:676646c50aeb 562 }
sjoerdbarts 8:676646c50aeb 563 else
sjoerdbarts 8:676646c50aeb 564 {
sjoerdbarts 8:676646c50aeb 565 motor2_pwm.write(abs(MotorValue));
sjoerdbarts 8:676646c50aeb 566 }
sjoerdbarts 8:676646c50aeb 567 }
sjoerdbarts 8:676646c50aeb 568 }
sjoerdbarts 8:676646c50aeb 569
sjoerdbarts 8:676646c50aeb 570 void BlinkLed(){
sjoerdbarts 8:676646c50aeb 571 led = not led;
sjoerdbarts 8:676646c50aeb 572 }
sjoerdbarts 8:676646c50aeb 573
sjoerdbarts 8:676646c50aeb 574 void Controller_motor()
sjoerdbarts 8:676646c50aeb 575 {
sjoerdbarts 8:676646c50aeb 576 // get the actual position
sjoerdbarts 8:676646c50aeb 577 double m1_Position = m1_GetPosition_cal();
sjoerdbarts 8:676646c50aeb 578 double m2_Position = m2_GetPosition_cal();
sjoerdbarts 8:676646c50aeb 579 // Set position scopes
sjoerdbarts 8:676646c50aeb 580 scope.set(0,x);
sjoerdbarts 8:676646c50aeb 581 scope.set(1,y);
sjoerdbarts 8:676646c50aeb 582 scope.set(2,Theta1);
sjoerdbarts 8:676646c50aeb 583 scope.set(3,Theta2);
sjoerdbarts 8:676646c50aeb 584 /*
sjoerdbarts 8:676646c50aeb 585 scope.set(0,Theta1);
sjoerdbarts 8:676646c50aeb 586 scope.set(1,m1_Position);
sjoerdbarts 8:676646c50aeb 587 scope.set(3,Theta2);
sjoerdbarts 8:676646c50aeb 588 scope.set(4,m2_Position);
sjoerdbarts 8:676646c50aeb 589 */
sjoerdbarts 8:676646c50aeb 590 //scope.set(2,m1_Position);
sjoerdbarts 8:676646c50aeb 591 //scope.set(4,m2_Position);
sjoerdbarts 8:676646c50aeb 592 // calc the error
sjoerdbarts 8:676646c50aeb 593 double m1_error = Theta1 - m1_Position;
sjoerdbarts 8:676646c50aeb 594 double m2_error = Theta2 - m2_Position;
sjoerdbarts 8:676646c50aeb 595 //scope.set(2,m1_error);
sjoerdbarts 8:676646c50aeb 596 //scope.set(6,m2_error);
sjoerdbarts 8:676646c50aeb 597 // calc motorvalues for controller;
sjoerdbarts 8:676646c50aeb 598 double m1_MotorValue = m1_PID(m1_error, Kp, Ki, Kd, Ts, N, m1_v1, m1_v2);
sjoerdbarts 8:676646c50aeb 599 double m2_MotorValue = m2_PID(m2_error, Kp, Ki, Kd, Ts, N, m2_v1, m2_v2);
sjoerdbarts 8:676646c50aeb 600 scope.set(4,m1_MotorValue);
sjoerdbarts 8:676646c50aeb 601 scope.set(5,m2_MotorValue);
sjoerdbarts 8:676646c50aeb 602 // Set the motorvalues
sjoerdbarts 18:32e9db0d47c9 603 SetMotor(1, m1_MotorValue);
sjoerdbarts 18:32e9db0d47c9 604 SetMotor(2, m2_MotorValue);
sjoerdbarts 8:676646c50aeb 605 // Set motorvalues for scope
sjoerdbarts 8:676646c50aeb 606 // Send data to HIDScope
sjoerdbarts 8:676646c50aeb 607 scope.send();
sjoerdbarts 8:676646c50aeb 608 }
sjoerdbarts 8:676646c50aeb 609
sjoerdbarts 8:676646c50aeb 610 void PotControl()
sjoerdbarts 8:676646c50aeb 611 {
sjoerdbarts 19:3af738c38db0 612 double Motor1_Value = (pot1.read() - 0.5f)*1.5f;
sjoerdbarts 19:3af738c38db0 613 double Motor2_Value = (pot2.read() - 0.5f)*1.5f;
sjoerdbarts 8:676646c50aeb 614 //pc.printf("\r\n Motor value 1: %f \r\n",Motor1_Value);
sjoerdbarts 8:676646c50aeb 615 //pc.printf("\r\n Motor value 2: %f \r\n",Motor2_Value);
sjoerdbarts 8:676646c50aeb 616 double m1_Position = m1_GetPosition();
sjoerdbarts 8:676646c50aeb 617 double m2_Position = m2_GetPosition();
sjoerdbarts 8:676646c50aeb 618 scope.set(0, Motor1_Value);
sjoerdbarts 8:676646c50aeb 619 scope.set(1, m1_Position);
sjoerdbarts 8:676646c50aeb 620 scope.set(2, Motor2_Value);
sjoerdbarts 8:676646c50aeb 621 scope.set(3, m2_Position);
sjoerdbarts 8:676646c50aeb 622 scope.send();
sjoerdbarts 8:676646c50aeb 623 // Write the motors
sjoerdbarts 8:676646c50aeb 624 SetMotor(1, Motor1_Value);
sjoerdbarts 8:676646c50aeb 625 SetMotor(2, Motor2_Value);
sjoerdbarts 8:676646c50aeb 626 }
sjoerdbarts 8:676646c50aeb 627
sjoerdbarts 8:676646c50aeb 628 void ResetEncoders()
sjoerdbarts 8:676646c50aeb 629 {
sjoerdbarts 8:676646c50aeb 630 m1_EncoderCW.reset();
sjoerdbarts 8:676646c50aeb 631 m1_EncoderCCW.reset();
sjoerdbarts 8:676646c50aeb 632 m2_EncoderCW.reset();
sjoerdbarts 8:676646c50aeb 633 m2_EncoderCCW.reset();
sjoerdbarts 8:676646c50aeb 634 }
sjoerdbarts 8:676646c50aeb 635
sjoerdbarts 8:676646c50aeb 636 void Button1Switch()
sjoerdbarts 8:676646c50aeb 637 {
sjoerdbarts 8:676646c50aeb 638 button1_value = not button1_value;
sjoerdbarts 15:e9439eac1342 639 pc.printf("\r\n Button1_value = %d \r\n", button1_value);
sjoerdbarts 8:676646c50aeb 640 }
sjoerdbarts 8:676646c50aeb 641
sjoerdbarts 8:676646c50aeb 642 void Button2Switch()
sjoerdbarts 8:676646c50aeb 643 {
sjoerdbarts 8:676646c50aeb 644 button2_value = not button2_value;
sjoerdbarts 15:e9439eac1342 645 pc.printf("\r\n Button2_value = %d \r\n", button2_value);
s1600907 1:e1267e72ade8 646 }
s1600907 0:c96cf9760185 647
sjoerdbarts 13:e8b8b035abbd 648 void PANIC()
sjoerdbarts 13:e8b8b035abbd 649 {
sjoerdbarts 13:e8b8b035abbd 650 CalculationsTicker.detach();
sjoerdbarts 13:e8b8b035abbd 651 PIDControlTicker.detach();
sjoerdbarts 15:e9439eac1342 652 Stepper_Off();
sjoerdbarts 13:e8b8b035abbd 653 SetMotor(1,0.0);
sjoerdbarts 13:e8b8b035abbd 654 SetMotor(2,0.0);
sjoerdbarts 13:e8b8b035abbd 655 pc.printf("\r\n PANIC BUTTON PRESSED \r\n");
sjoerdbarts 13:e8b8b035abbd 656 pc.printf("\r\n TERMINATING ALL PROCESSES \r\n");
sjoerdbarts 13:e8b8b035abbd 657 }
sjoerdbarts 13:e8b8b035abbd 658
s1600907 0:c96cf9760185 659 ////////////////////////// main loop ////////////////////////////////////////////
s1600907 0:c96cf9760185 660 int main()
sjoerdbarts 8:676646c50aeb 661 {
sjoerdbarts 8:676646c50aeb 662 // Setup
sjoerdbarts 8:676646c50aeb 663 // Set baud connection with PC
s1600907 0:c96cf9760185 664 pc.baud(SERIAL_BAUD);
sjoerdbarts 8:676646c50aeb 665 pc.printf("\r\n ***THERMONUCLEAR WARFARE COMMENCES*** \r\n");
sjoerdbarts 18:32e9db0d47c9 666 pc.printf("\r\n *** PROGRAM: CompleteFunction *** \r\n");
s1600907 0:c96cf9760185 667
s1600907 0:c96cf9760185 668 /* making the biquad chain for filtering the emg
s1600907 0:c96cf9760185 669 notch and high pass */
s1600907 0:c96cf9760185 670 bqc.add( &bq1 ).add( &bq2 );
s1600907 0:c96cf9760185 671 bqc2.add( &bq4 ).add( &bq5 );
s1600907 0:c96cf9760185 672 bqc3.add( &bq7 ).add( &bq8 );
sjoerdbarts 13:e8b8b035abbd 673
sjoerdbarts 8:676646c50aeb 674 // Setup Blinking LED to blink every 0.5 sec
sjoerdbarts 8:676646c50aeb 675 led = 1;
sjoerdbarts 8:676646c50aeb 676 BlinkLedTicker.attach(BlinkLed,0.5);
sjoerdbarts 8:676646c50aeb 677
sjoerdbarts 14:f114e57c3795 678 // Setup other leds
sjoerdbarts 14:f114e57c3795 679 red = 1;
sjoerdbarts 14:f114e57c3795 680 blue = 1;
sjoerdbarts 14:f114e57c3795 681 green = 1;
sjoerdbarts 14:f114e57c3795 682
sjoerdbarts 8:676646c50aeb 683 // Setup motor PWM speeds
sjoerdbarts 8:676646c50aeb 684 motor1_pwm.period(1.0/1000);
sjoerdbarts 8:676646c50aeb 685 motor2_pwm.period(1.0/1000);
sjoerdbarts 7:bafc32b576c4 686
sjoerdbarts 8:676646c50aeb 687 // Setup Interruptin.fall
sjoerdbarts 8:676646c50aeb 688 button1.fall(Button1Switch);
sjoerdbarts 13:e8b8b035abbd 689 button2.fall(PANIC);
sjoerdbarts 13:e8b8b035abbd 690 button3.fall(Stepper_On);
sjoerdbarts 8:676646c50aeb 691 //button4.fall(xxx);
s1600907 0:c96cf9760185 692
sjoerdbarts 8:676646c50aeb 693 // Actual code starts here
sjoerdbarts 8:676646c50aeb 694 // Start positioning the arms
sjoerdbarts 19:3af738c38db0 695 pc.printf("\r\n ***************** \r");
sjoerdbarts 19:3af738c38db0 696 pc.printf("\r\n Press Button 1 to start positioning the arms using PotControl\r");
sjoerdbarts 19:3af738c38db0 697 pc.printf("\r\n Make sure both potentiometers are positioned halfway! \r");
sjoerdbarts 8:676646c50aeb 698 pc.printf("\r\n ***************** \r\n");
sjoerdbarts 18:32e9db0d47c9 699 //pc.printf("\r\n Button1_value = %d \r\n", button1_value);
sjoerdbarts 15:e9439eac1342 700 while (button1_value == false)
sjoerdbarts 15:e9439eac1342 701 {
sjoerdbarts 19:3af738c38db0 702 wait(2);
sjoerdbarts 18:32e9db0d47c9 703 //pc.printf("\r\n Button1_value = %d \r\n", button1_value);
sjoerdbarts 18:32e9db0d47c9 704 //wait(5);
sjoerdbarts 15:e9439eac1342 705 }
sjoerdbarts 8:676646c50aeb 706 PotControlTicker.attach(&PotControl,CONTROLLER_TS);
s1600907 0:c96cf9760185 707
sjoerdbarts 19:3af738c38db0 708 pc.printf("\r\n ***************** \r");
sjoerdbarts 8:676646c50aeb 709 pc.printf("\r\n When done positioning, press Button 1 to detach Potcontrol");
sjoerdbarts 8:676646c50aeb 710 pc.printf("\r\n ***************** \r\n");
sjoerdbarts 15:e9439eac1342 711 while (button1_value == true)
sjoerdbarts 15:e9439eac1342 712 {
sjoerdbarts 18:32e9db0d47c9 713 //pc.printf("\r\n Button1_value = %d \r\n", button1_value);
sjoerdbarts 19:3af738c38db0 714 wait(2);
sjoerdbarts 15:e9439eac1342 715 }
sjoerdbarts 8:676646c50aeb 716
sjoerdbarts 8:676646c50aeb 717 // Detach potcontrol
sjoerdbarts 8:676646c50aeb 718 PotControlTicker.detach();
sjoerdbarts 8:676646c50aeb 719
sjoerdbarts 8:676646c50aeb 720 // Set motors to 0
sjoerdbarts 8:676646c50aeb 721 SetMotor(1,0.0);
sjoerdbarts 8:676646c50aeb 722 SetMotor(2,0.0);
sjoerdbarts 8:676646c50aeb 723 // Wait a bit to let the movement stop
sjoerdbarts 8:676646c50aeb 724 wait(0.5);
sjoerdbarts 8:676646c50aeb 725 // Reset the encoders to set the 0 position
sjoerdbarts 8:676646c50aeb 726 ResetEncoders();
sjoerdbarts 8:676646c50aeb 727
sjoerdbarts 8:676646c50aeb 728 // PID control starts
sjoerdbarts 19:3af738c38db0 729 pc.printf("\r\n ***************** \r");
sjoerdbarts 19:3af738c38db0 730 pc.printf("\r\n When done positioning, press Button 1 to start potmeter PID control");
sjoerdbarts 19:3af738c38db0 731 pc.printf("\r\n Make sure both potentiometers are positioned halfway!!!");
sjoerdbarts 8:676646c50aeb 732 pc.printf("\r\n ***************** \r\n");
sjoerdbarts 15:e9439eac1342 733 while (button1_value == false)
sjoerdbarts 15:e9439eac1342 734 {
sjoerdbarts 18:32e9db0d47c9 735 //pc.printf("\r\n Button1_value = %d \r\n", button1_value);
sjoerdbarts 19:3af738c38db0 736 wait(2);
sjoerdbarts 15:e9439eac1342 737 }
sjoerdbarts 8:676646c50aeb 738
sjoerdbarts 8:676646c50aeb 739 // Attach sample, calc and control tickers
sjoerdbarts 8:676646c50aeb 740 CalculationsTicker.attach(&CalculationsForTheta,CALC_TS);
sjoerdbarts 12:9b0451105991 741 PIDControlTicker.attach(&Controller_motor, CONTROLLER_TS);
sjoerdbarts 8:676646c50aeb 742 while(true){}
s1600907 0:c96cf9760185 743 }