Thomas Burgers / Mbed 2 deprecated ZZ-TheChenneRobot

Dependencies:   Encoder HIDScope MODSERIAL QEI biquadFilter mbed

Committer:
ThomasBNL
Date:
Thu Oct 08 20:49:58 2015 +0000
Revision:
27:b7caf668a682
Parent:
26:b3693f431d6f
Child:
28:482ac040fb0d
REGELAAR begin:   EMG--> REGELAAR -> CONTROLLER

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ThomasBNL 0:40052f5ca77b 1 #include "mbed.h"
ThomasBNL 21:c75210216204 2 #include "HIDScope.h"
ThomasBNL 0:40052f5ca77b 3 #include "QEI.h"
ThomasBNL 0:40052f5ca77b 4 #include "MODSERIAL.h"
ThomasBNL 8:50d6e2323d3b 5 #include "biquadFilter.h"
ThomasBNL 0:40052f5ca77b 6 #include "encoder.h"
ThomasBNL 0:40052f5ca77b 7
ThomasBNL 24:8ca471562212 8 MODSERIAL pc(USBTX,USBRX);
ThomasBNL 24:8ca471562212 9 DigitalOut debug_led_red(LED_RED); // Debug LED
ThomasBNL 24:8ca471562212 10 DigitalOut debug_led_green(LED_GREEN); // Debug LED
ThomasBNL 25:d536056a2666 11 DigitalOut debug_led_blue(LED_BLUE); // Debug LED
ThomasBNL 8:50d6e2323d3b 12
ThomasBNL 27:b7caf668a682 13 //HIDScope scope(2); // HIDSCOPE declared
ThomasBNL 14:599896acf576 14
ThomasBNL 8:50d6e2323d3b 15 // (DEBUGGING AND TESTING BUTTONS) (0 when pressed and 1 when not pressed)
ThomasBNL 8:50d6e2323d3b 16 DigitalIn buttonL1(PTC6); // Button 1 (laag op board) for testing at the lower board
ThomasBNL 8:50d6e2323d3b 17 DigitalIn buttonL2(PTA4); // Button 2 (laag op board) for testing at the lower board
ThomasBNL 8:50d6e2323d3b 18 DigitalIn buttonH1(D2); // Button 3 (hoog op board) for testing at the top board
ThomasBNL 8:50d6e2323d3b 19 DigitalIn buttonH2(D6); // Button 4 (hoog op board) for testing at the top board
ThomasBNL 0:40052f5ca77b 20
ThomasBNL 27:b7caf668a682 21 //volatile bool looptimerflag;
ThomasBNL 8:50d6e2323d3b 22 const double cw=0; // zero is clockwise (front view)
ThomasBNL 8:50d6e2323d3b 23 const double ccw=1; // one is counterclockwise (front view)
ThomasBNL 8:50d6e2323d3b 24
ThomasBNL 8:50d6e2323d3b 25 const double sample_time=0.01; // tijd voor een sample (100Hz)
ThomasBNL 27:b7caf668a682 26
ThomasBNL 7:ddd7fb357786 27 // Functions used (described after main)
ThomasBNL 27:b7caf668a682 28 // void setlooptimerflag(void);
ThomasBNL 8:50d6e2323d3b 29
ThomasBNL 0:40052f5ca77b 30
ThomasBNL 7:ddd7fb357786 31 // MAIN function
ThomasBNL 0:40052f5ca77b 32 int main() {
ThomasBNL 27:b7caf668a682 33 debug_led_red=0;
ThomasBNL 27:b7caf668a682 34 debug_led_blue=0;
ThomasBNL 27:b7caf668a682 35 debug_led_green=0;
ThomasBNL 27:b7caf668a682 36 AnalogIn potmeter1(A0); // POTMETER -> DOEN ALSOF EMG SIGNAAL
ThomasBNL 27:b7caf668a682 37 AnalogIn potmeter2(A1); // POTMETER -> DOEN ALSOF EMG SIGNAAL
ThomasBNL 27:b7caf668a682 38
ThomasBNL 8:50d6e2323d3b 39 QEI motor_turn(D12,D13,NC,32); // Encoder for motor Turn
ThomasBNL 8:50d6e2323d3b 40 PwmOut pwm_motor_turn(D5); // Pwm for motor Turn
ThomasBNL 8:50d6e2323d3b 41 DigitalOut motordirection_turn(D4); // Direction of the motor Turn
ThomasBNL 27:b7caf668a682 42 double reference_turn=0; // Set constant to store reference value of the Turn motor
ThomasBNL 8:50d6e2323d3b 43 double position_turn; // Set constant to store current position of the Turn motor
ThomasBNL 8:50d6e2323d3b 44 double error;
ThomasBNL 10:09ba965045a7 45 double pwm_to_motor_turn;
ThomasBNL 10:09ba965045a7 46 double pwm_motor_turn_P;
ThomasBNL 10:09ba965045a7 47 double pwm_motor_turn_I;
ThomasBNL 10:09ba965045a7 48 double pwm_motor_turn_D;
ThomasBNL 0:40052f5ca77b 49
ThomasBNL 7:ddd7fb357786 50 //START OF CODE
ThomasBNL 7:ddd7fb357786 51 pc.printf("Start of code \n\r");
ThomasBNL 0:40052f5ca77b 52
ThomasBNL 23:335a3b843a5e 53 pc.baud(115200); // Set the baudrate
ThomasBNL 0:40052f5ca77b 54
ThomasBNL 7:ddd7fb357786 55 // Tickers
ThomasBNL 8:50d6e2323d3b 56 Ticker looptimer; // Ticker called looptimer to set a looptimerflag
ThomasBNL 8:50d6e2323d3b 57 looptimer.attach(setlooptimerflag,sample_time); // calls the looptimer flag every 0.01s
ThomasBNL 0:40052f5ca77b 58
ThomasBNL 22:eaf4cbd1dcec 59 //Sample_Ticker.attach(&get_sample, sample_time); // HIDSCOPE sample Ticker
ThomasBNL 14:599896acf576 60
ThomasBNL 7:ddd7fb357786 61 pc.printf("Start infinite loop \n\r");
ThomasBNL 16:a8d2c721cf56 62 wait (3); // Wait before starting system
ThomasBNL 0:40052f5ca77b 63
ThomasBNL 0:40052f5ca77b 64 //INFINITE LOOP
ThomasBNL 5:8fb74a22fe3c 65 while(1)
ThomasBNL 27:b7caf668a682 66 { // Start while loop}
ThomasBNL 8:50d6e2323d3b 67 // Wait until looptimer flag is true then execute PID controller.
ThomasBNL 0:40052f5ca77b 68 while(looptimerflag != true);
ThomasBNL 0:40052f5ca77b 69 looptimerflag = false;
ThomasBNL 0:40052f5ca77b 70
ThomasBNL 27:b7caf668a682 71 // INPUT EMG SIGNAAL
ThomasBNL 27:b7caf668a682 72 EMG_R = (potmeter1.read()-0.5)*2000; //EMG Right bicep
ThomasBNL 27:b7caf668a682 73 EMG_L = (potmeter2.read()-0.5)*2000; // EMG Left bicep
ThomasBNL 27:b7caf668a682 74
ThomasBNL 27:b7caf668a682 75 // calibratie
ThomasBNL 27:b7caf668a682 76 double max_EMG = 40; // Calibreren
ThomasBNL 27:b7caf668a682 77 double min_EMG = 0;
ThomasBNL 27:b7caf668a682 78 double Hit=60; // position when bottle is hit
ThomasBNL 0:40052f5ca77b 79
ThomasBNL 27:b7caf668a682 80 // REGELAAR (wat gaat de controller doen om bestuurd te worden)
ThomasBNL 27:b7caf668a682 81 const double Threshold_Bicep_Left_1=10; // percentages van min en max EMG
ThomasBNL 27:b7caf668a682 82 const double Threshold_Bicep_Left_2=20;
ThomasBNL 27:b7caf668a682 83 const double Threshold_Bicep_Left_3=30;
ThomasBNL 27:b7caf668a682 84 const double Threshold_Bicep_Right_1=10;
ThomasBNL 27:b7caf668a682 85 const double Threshold_Bicep_Right_2=20;
ThomasBNL 3:11a7da46e093 86
ThomasBNL 27:b7caf668a682 87 Nieuwe_actie: // Komt hier weer terecht na
ThomasBNL 3:11a7da46e093 88
ThomasBNL 0:40052f5ca77b 89
ThomasBNL 27:b7caf668a682 90 // SLAG
ThomasBNL 27:b7caf668a682 91 if (EMG_L > Threshold_Bicep_Left_1 || EMG_R > Threshold_Bicep_Right_1)
ThomasBNL 27:b7caf668a682 92 {
ThomasBNL 27:b7caf668a682 93 wait(0.5); //(wacht 0.5s om te kijken hoe hard geslagen moet worden)
ThomasBNL 27:b7caf668a682 94 while(EMG_L > Threshold_Bicep_Left_1 || EMG_R > Threshold_Bicep_Right_1) // Nog steeds over de threshold?
ThomasBNL 27:b7caf668a682 95 {
ThomasBNL 27:b7caf668a682 96 // deactivate_PID_Controller_strike // function that sets P, I and D gain values to zero
ThomasBNL 27:b7caf668a682 97 wait(0.1); // wacht 10 samples
ThomasBNL 27:b7caf668a682 98 pwm_to_motor_strike=((EMG_L+EMG_R)/(max_EMG+min_EMG))*0.7 + 0.3 // min speed 0.3 en max 1
ThomasBNL 27:b7caf668a682 99 motor_strike_direction=cw; // towards bottle
ThomasBNL 11:ecd83b303252 100
ThomasBNL 27:b7caf668a682 101 if((Position-Hit)<2) // geraakt als position-hit <2 graden
ThomasBNL 27:b7caf668a682 102 (
ThomasBNL 27:b7caf668a682 103 // activate_PID_Controller_strike // function sets back P, I and D gain values
ThomasBNL 27:b7caf668a682 104 goto Nieuwe_actie; // kunt weer iets nieuws doen indien vorige actie is uitgevoerd
ThomasBNL 27:b7caf668a682 105 }
ThomasBNL 27:b7caf668a682 106 }
ThomasBNL 27:b7caf668a682 107
ThomasBNL 27:b7caf668a682 108 // activate_PID_Controller_strike // function sets back P, I and D gain values
ThomasBNL 27:b7caf668a682 109 // want het kan zijn dat bovenste script half wordt uitgevoerd en dan moet het slag mechanisme wel weer terug
ThomasBNL 3:11a7da46e093 110
ThomasBNL 27:b7caf668a682 111 // DRAAI LINKS
ThomasBNL 27:b7caf668a682 112 else if (EMG_L > Threshold_Bicep_Left_2 || EMG_R < Threshold_Bicep_Right_1)
ThomasBNL 27:b7caf668a682 113 {
ThomasBNL 27:b7caf668a682 114
ThomasBNL 27:b7caf668a682 115 n=0;
ThomasBNL 27:b7caf668a682 116 while(EMG_L > Threshold_Bicep_Left_1 || EMG_R < Threshold_Bicep_Right_1)
ThomasBNL 27:b7caf668a682 117 {
ThomasBNL 27:b7caf668a682 118 if (n==0)
ThomasBNL 27:b7caf668a682 119 {
ThomasBNL 27:b7caf668a682 120 Reference=Reference+45;
ThomasBNL 27:b7caf668a682 121 n=1;
ThomasBNL 27:b7caf668a682 122 } //(45 graden naar links voor volgende fles) //wordt maar 1 keer uitgevoerd
ThomasBNL 27:b7caf668a682 123
ThomasBNL 27:b7caf668a682 124 if (abs(error)<2) // als error en kleiner dan twee graden
ThomasBNL 27:b7caf668a682 125 {
ThomasBNL 27:b7caf668a682 126 wait(0.5);
ThomasBNL 27:b7caf668a682 127 if (abs(error)<2) // Is de error na 0.5 seconde nog steeds kleiner dan twee graden?
ThomasBNL 27:b7caf668a682 128 {
ThomasBNL 27:b7caf668a682 129 goto Nieuwe_actie; // kunt weer iets nieuws doen indien vorige actie is uitgevoerd
ThomasBNL 27:b7caf668a682 130 }
ThomasBNL 27:b7caf668a682 131 }
ThomasBNL 27:b7caf668a682 132 }
ThomasBNL 3:11a7da46e093 133 }
ThomasBNL 8:50d6e2323d3b 134
ThomasBNL 27:b7caf668a682 135 // DRAAI RECHTS
ThomasBNL 27:b7caf668a682 136 else if (EMG_R > Threshold_Bicep_Left_2 || EMG_L < Threshold_Bicep_Right_1)
ThomasBNL 27:b7caf668a682 137 {
ThomasBNL 27:b7caf668a682 138 n=0;
ThomasBNL 27:b7caf668a682 139 while(EMG_R > Threshold_Bicep_Left_1 || EMG_L < Threshold_Bicep_Right_1)
ThomasBNL 27:b7caf668a682 140 {
ThomasBNL 27:b7caf668a682 141 if (n==0)
ThomasBNL 27:b7caf668a682 142 {
ThomasBNL 27:b7caf668a682 143 Reference=Reference-45;
ThomasBNL 27:b7caf668a682 144 n=1;
ThomasBNL 27:b7caf668a682 145 } //(45 graden naar rechts voor volgende fles) //wordt maar 1 keer uitgevoerd
ThomasBNL 27:b7caf668a682 146
ThomasBNL 27:b7caf668a682 147 if (abs(error)<2) // als error en kleiner dan twee graden
ThomasBNL 27:b7caf668a682 148 {
ThomasBNL 27:b7caf668a682 149 wait(0.5);
ThomasBNL 27:b7caf668a682 150 if (abs(error)<2) // Is de error na 0.5 seconde nog steeds kleiner dan twee graden?
ThomasBNL 27:b7caf668a682 151 {
ThomasBNL 27:b7caf668a682 152 goto Nieuwe_actie; // kunt weer iets nieuws doen indien vorige actie is uitgevoerd
ThomasBNL 27:b7caf668a682 153 }
ThomasBNL 27:b7caf668a682 154 }
ThomasBNL 27:b7caf668a682 155 }
ThomasBNL 27:b7caf668a682 156 }
ThomasBNL 27:b7caf668a682 157
ThomasBNL 27:b7caf668a682 158 // Send HIDSCOPE data
ThomasBNL 27:b7caf668a682 159 // scope.set(0,reference_turn); // HIDSCOPE channel 0 : Current Reference
ThomasBNL 27:b7caf668a682 160 // scope.set(0,position_turn); // HIDSCOPE channel 0 : Position_turn
ThomasBNL 27:b7caf668a682 161 // scope.set(1,pwm_to_motor_turn); // HIDSCOPE channel 1 : Pwm_to_motor_turn
ThomasBNL 27:b7caf668a682 162 // scope.send(); // Send channel info to HIDSCOPE
ThomasBNL 22:eaf4cbd1dcec 163 // }
ThomasBNL 0:40052f5ca77b 164 }
ThomasBNL 0:40052f5ca77b 165 }
ThomasBNL 5:8fb74a22fe3c 166 }
ThomasBNL 0:40052f5ca77b 167
ThomasBNL 0:40052f5ca77b 168 // Keep in range function
ThomasBNL 0:40052f5ca77b 169 void keep_in_range(double * in, double min, double max)
ThomasBNL 0:40052f5ca77b 170 {
ThomasBNL 0:40052f5ca77b 171 *in > min ? *in < max? : *in = max: *in = min;
ThomasBNL 0:40052f5ca77b 172 }
ThomasBNL 0:40052f5ca77b 173
ThomasBNL 0:40052f5ca77b 174 // Looptimerflag function
ThomasBNL 0:40052f5ca77b 175 void setlooptimerflag(void)
ThomasBNL 0:40052f5ca77b 176 {
ThomasBNL 0:40052f5ca77b 177 looptimerflag = true;
ThomasBNL 1:dc683e88b44e 178 }
ThomasBNL 1:dc683e88b44e 179
ThomasBNL 22:eaf4cbd1dcec 180 //// Get sample
ThomasBNL 22:eaf4cbd1dcec 181 //void get_sample(void) // HIDSCOPE sample fuction
ThomasBNL 22:eaf4cbd1dcec 182 //{
ThomasBNL 22:eaf4cbd1dcec 183 // sample = true;
ThomasBNL 22:eaf4cbd1dcec 184 //}