main.cpp

00001 /********************************************/
00002 /*                                          */
00003 /*   BRONCODE GROEP 7, MODULE 9, 2014       */
00004 /*       *-THE SLAP-*                       */
00005 /*                                          */
00006 /* -Anne ten Dam                            */
00007 /* -Laura de Heus                           */
00008 /* -Moniek Strijdveen                       */
00009 /* -Bart Arendshorst                        */
00010 /* -Peter Bartels                           */
00011 /********************************************/
00012 
00013 /*
00014 #include voor alle libraries
00015 */
00016 #include "TextLCD.h"
00017 #include "mbed.h"
00018 #include "encoder.h"
00019 #include "HIDScope.h"
00020 #include "PwmOut.h"
00021 #include "arm_math.h"
00022 
00023 /*
00024 #define vaste waarden
00025 */
00026 /*definieren pinnen Motor 1*/
00027 #define M1_PWM PTA5
00028 #define M1_DIR PTA4
00029 #define M2_PWM PTC8
00030 #define M2_DIR PTC9
00031 /*Definieren minimale waarden PWM per motor*/
00032 #define PWM1_min_50 0.529 /*met koppelstuk!*/
00033 #define PWM2_min_30 0.529 /*aanpassen! Klopt nog niet met koppelstuk*/
00034 /*Definieren om de hoeveel seconden er gegevens naar de HID-scope gestuurd worden.*/
00035 #define TSAMP 0.005
00036 #define K_P (5)
00037 #define K_I (0.1 * TSAMP)
00038 #define K_D (0)
00039 //#define K_D (0.0005 /TSAMP)
00040 #define I_LIMIT 100.
00041 #define PI 3.1415926535897
00042 #define lengte_arm 0.5
00043 
00044 /*
00045 Geef een naam aan een actie en vertel welke pinnen hiervoor gebruikt worden.
00046 */
00047 TextLCD lcd(PTD2, PTA12, PTB2, PTB3, PTC2, PTA13, TextLCD::LCD16x2); // rs, e, d4-d7
00048 Encoder motor1(PTD3,PTD1);
00049 Encoder motor2(PTD5, PTD0); 
00050 PwmOut pwm_motor1(M1_PWM);
00051 PwmOut pwm_motor2(M2_PWM); 
00052 DigitalOut motordir1(M1_DIR);
00053 DigitalOut motordir2(M2_DIR); 
00054 DigitalOut LEDGREEN(LED_GREEN);
00055 DigitalOut LEDRED(LED_RED); 
00056 Serial pc(USBTX,USBRX);
00057 HIDScope scope(3);
00058 AnalogIn    emg(PTB1);
00059 /*
00060 definieer namen aan var, float, int, static float, uint8_t, uint16_t etc. en geef ze eventueel een waarde
00061 */
00062 Ticker statemachine;
00063 Ticker screen;
00064 arm_biquad_casd_df1_inst_f32 lowpass_1; //2e orde lowpass biquad butterworthfilter 99Hz
00065 arm_biquad_casd_df1_inst_f32 lowpass_2; //2e orde lowpass biquad butterworthfilter 3Hz
00066 arm_biquad_casd_df1_inst_f32 highpass; //2e orde highpass biquad butterworthfilter 20Hz
00067 arm_biquad_casd_df1_inst_f32 notch; //2e orde lowpass biquad butterworthfilter 50Hz
00068 int previous_herhalingen = 0;
00069 int current_herhalingen = 0;
00070 int PWM2_percentage = 100; 
00071 int aantal_rotaties_1 = 10;
00072 int aantalcr_1 = 1600;
00073 int aantalcr_2 = 960; 
00074 int beginpos_motor1;
00075 int beginpos_motor1_new; 
00076 int beginpos_motor2;
00077 int beginpos_motor2_new;
00078 int previous_pos_motor1 = 0;
00079 int current_pos_motor1;
00080 int EMG = 1;
00081 int delta_pos_motor1_puls;
00082 int aantal_pieken;
00083 int doel;
00084 int doel_richting;
00085 int doel_hoogte;
00086 bool aanspan;
00087 void clamp(float * in, float min, float max);
00088 volatile bool looptimerflag;
00089 int16_t gewenste_snelheid = 2;
00090 int16_t gewenste_snelheid_rad = 4; 
00091 int16_t gewenste_snelheid_rad_return = 1;
00092 float pid(float setpoint, float measurement);
00093 float pos_motor1_rad;
00094 float PWM1_percentage = 0;
00095 float delta_pos_motor1_rad;
00096 float snelheid_motor1_rad;
00097 float snelheid_arm_ms; 
00098 float PWM1; 
00099 float PWM2;
00100 float Speed_motor1;
00101 float Speed_motor1rad;
00102 float setpoint = 0;
00103 float prev_setpoint = 0;
00104 float lowpass_1_const[] = {0.978030479206560 , 1.956060958413119 , 0.978030479206560 , -1.955578240315036 , -0.956543676511203};
00105 float lowpass_1_states[4];
00106 float lowpass_2_const[] = {0.002080567135492 , 0.004161134270985 , 0.002080567135492 , 1.866892279711715 , -0.875214548253684};
00107 float lowpass_2_states[4];
00108 float highpass_const[] = {0.638945525159022 , -1.277891050318045 ,  0.638945525159022 , 1.142980502539901 , -0.412801598096189};
00109 float highpass_states[4];
00110 float notch_const[] = {0.978048948305681 , 0.000000000000000 , 0.978048948305681 , 0.000000000000000 , -0.956097896611362};
00111 float notch_states[4];
00112 float emg_filtered;
00113 float emg_max = 0;
00114 float emg_treshhold_laag = 0;
00115 float emg_treshhold_hoog = 0;
00116 
00117 //HIDScope scope(6);
00118 
00119 enum  state_t {RUST, EMG_KALIBRATIE, ARM_KALIBRATIE, METEN_HOOGTE, METEN_RICHTING, INSTELLEN_RICHTING, SLAAN, RETURN2RUST}; //verschillende stadia definieren voor gebruik in CASES
00120 state_t state = RUST;
00121 
00122 //functies die vanuit de statemachinefunction aangeroepen kunnen worden
00123 void rust() {
00124     current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
00125 }
00126     
00127 void pieken_tellen(){
00128     if (emg_filtered>=emg_treshhold_hoog) 
00129     {
00130         aanspan=true; //maak een variabele waarin je opslaat dat het signaal hoog is.
00131     }
00132     if (aanspan==true && emg_filtered<=emg_treshhold_laag)//== ipv =, anders wordt aanspan true gemaakt
00133     {
00134         aanspan=false;
00135         aantal_pieken++;
00136         doel = aantal_pieken-((aantal_pieken/3)*3)+1;
00137     }
00138 }
00139 
00140 void emg_filtering() {
00141     uint16_t emg_value;
00142     float emg_value_f32;
00143     emg_value = emg.read_u16(); // read direct ADC result, converted to 16 bit integer (0..2^16 = 0..65536 = 0..3.3V)
00144     emg_value_f32 = emg.read();
00145     
00146     arm_biquad_cascade_df1_f32(&highpass, &emg_value_f32, &emg_filtered, 1 );
00147     arm_biquad_cascade_df1_f32(&lowpass_1, &emg_filtered, &emg_filtered, 1 );
00148     arm_biquad_cascade_df1_f32(&notch, &emg_filtered, &emg_filtered, 1);
00149     emg_filtered = fabs(emg_filtered);
00150     arm_biquad_cascade_df1_f32(&lowpass_2, &emg_filtered, &emg_filtered, 1 );
00151     scope.set(0,emg_value);     //uint value
00152     scope.set(1,emg_filtered);  //processed float
00153     scope.set(2,doel);
00154     scope.send();
00155     if(state!=EMG_KALIBRATIE)
00156     {
00157         pieken_tellen();
00158     }
00159 }
00160 
00161 void emg_max_meting(){
00162     emg_filtering();
00163     if (emg_filtered>=emg_max)
00164     {
00165         emg_max=emg_filtered;
00166     }
00167     emg_treshhold_laag = 0.3*emg_max;
00168     emg_treshhold_hoog = 0.7*emg_max;
00169 }
00170 
00171 void emg_kalibratie() {
00172     //if(emg_filtered>=0.05){//Deze if-loop alleen zodat het  nog op de hidscope kan worden gezien, dit mag weg wanneer er een display is, current_herhalingen wel laten.
00173         current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
00174     //}
00175     if(current_herhalingen<=1000)
00176     {
00177         emg_max_meting();
00178     } 
00179 }
00180 
00181 void arm_kalibratie() {
00182     //voor nu om de loop te doorlopen wordt onderstaande code gebruikt. Nogmaal gesproken moet er gewacht worden op een 'hoog' signaal van een knop
00183     current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
00184     motor1.setPosition(0);
00185     motor2.setPosition(0);    
00186 }
00187 
00188 void doel_bepaling() {
00189     if(200<=current_herhalingen<=1200){
00190         emg_filtering();
00191     }
00192     else if(1600<=current_herhalingen<=2200){
00193         emg_filtering();        
00194     } 
00195     else{
00196     }    
00197 }
00198 
00199 void meten_hoogte() {
00200     current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
00201     doel_bepaling();
00202     if(1600<=current_herhalingen<=2200 && aantal_pieken==1){
00203         doel=doel_hoogte;
00204     }
00205 }
00206 
00207 void meten_richting() {
00208     current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
00209     doel_bepaling();
00210     if(1600<=current_herhalingen<=2200 && aantal_pieken==1){
00211         doel=doel_richting;
00212     }
00213 }
00214 
00215 void instellen_richting() {
00216     current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
00217 }
00218 
00219 void GotoPosition (float position_setpoint_rad, float speed_radpersecond) {
00220     
00221     current_pos_motor1 = motor1.getPosition(); //bekijk na elke 0.005s wat de huidige 'waarde' van de encoder is
00222     //delta_pos_motor1_puls = current_pos_motor1 - previous_pos_motor1; //Bereken wat het verschil in waarde is voor het berekenen van de snelheid(puls)
00223     pos_motor1_rad = current_pos_motor1/(1600/(2*PI)); //echte waarde omrekenen naar rad voor (positie) PID regelaar
00224     //delta_pos_motor1_rad = delta_pos_motor1_puls/(1600/(2*PI)); //delta waarde omrekenen naar rad voor snelheidsbepaling (rad)
00225     //snelheid_motor1_rad = delta_pos_motor1_rad / TSAMP; //rad delen door TSAMP om rad/s te verkrijgen
00226     //snelheid_arm_ms = snelheid_motor1_rad * lengte_arm; //deze waarde maar lengte van de arm om de snelheid van het uiteinde van die arm te verkrijgen
00227     //scope.set(0, snelheid_motor1_rad);
00228     
00229     previous_pos_motor1 = current_pos_motor1;  //sla de huidige waarde op als vorige waarde.
00230     
00231     //nu gaan we positie regelen i.p.v. snelheid.
00232     
00233     if (fabs(pos_motor1_rad - position_setpoint_rad) <= 0.05) //if position error < ...rad, then stop.
00234     {
00235         speed_radpersecond = 0; 
00236         setpoint = pos_motor1_rad;
00237         current_herhalingen = previous_herhalingen + 1; 
00238         previous_herhalingen = current_herhalingen;
00239         pc.printf("stop\n\r");
00240         PWM1_percentage = 0;
00241     }
00242     else if(pos_motor1_rad - position_setpoint_rad < 0)
00243     {    
00244         setpoint = prev_setpoint +( TSAMP * speed_radpersecond);
00245         PWM1_percentage = pid(setpoint, pos_motor1_rad);       
00246     }
00247     else
00248     {
00249         setpoint = prev_setpoint -( TSAMP * speed_radpersecond);
00250         PWM1_percentage = pid(setpoint, pos_motor1_rad);       
00251     }
00252     /*new_pwm = (setpoint - motor1.getPosition())*.001; -> P action*/
00253     pc.printf("%f\n\r",PWM1_percentage);
00254     
00255     
00256     //scope.set(1, setpoint-pos_motor1_rad); 
00257     
00258     if (PWM1_percentage < -100)
00259     {
00260         PWM1_percentage = -100;
00261     }
00262     else if (PWM1_percentage >100)
00263     {
00264         PWM1_percentage =100;
00265     }
00266     
00267     if(PWM1_percentage < 0)
00268     {
00269         motordir1 = 1;
00270     }
00271     else
00272     {
00273         motordir1 = 0;
00274     }
00275         
00276     pwm_motor1.write(abs(PWM1_percentage/100.));//abs(((1-PWM1_min_50)/100)*PWM1_percentage + PWM1_min_50));
00277     //scope.set(5, abs(((1-PWM1_min_50)/100)*PWM1_percentage + PWM1_min_50));
00278     prev_setpoint = setpoint;
00279     //scope.send();  
00280 }
00281 
00282 float pid(float setpoint, float measurement)
00283 {
00284     float error;
00285     static float prev_error = 0;
00286     float           out_p = 0;
00287     static float    out_i = 0;
00288     float           out_d = 0;
00289     error  = (setpoint-measurement);
00290     out_p  = error*K_P; 
00291     out_i += error*K_I;
00292     out_d  = (error-prev_error)*K_D;
00293     clamp(&out_i,-I_LIMIT,I_LIMIT);
00294     prev_error = error;
00295     //scope.set(2, out_p);
00296     //scope.set(3, out_i);
00297     //scope.set(4, out_d);
00298     return out_p + out_i + out_d;
00299 }
00300 
00301 void clamp(float* in, float min, float max) // "*" is een pointer (verwijst naar het adres waar een variebele instaat). Dus je slaat niet de variabele op
00302 // maar de locatie van de variabele. 
00303 {
00304     *in > min ? /*(*/*in < max? /*niets doen*/ : *in = max/*)*/: *in = min; // a ? b : c --> als a waar is, dan doe je b, en anders c 
00305     // *in = het getal dat staat op locatie van in --> waarde van new_pwm
00306 }
00307     
00308 void statemachinefunction()
00309 {
00310     switch(state) {
00311         case RUST: {
00312             rust();
00313             /*voorwaarde wanneer hij door kan naar de volgende case*/
00314             if (current_herhalingen == 100 && EMG == 1) 
00315             {
00316                 current_herhalingen = 0;
00317                 previous_herhalingen = 0;
00318                 state = ARM_KALIBRATIE;
00319                 EMG = 0;
00320             }
00321             break;
00322             /*bepalen hoe je zorgt dat je maar 1x de kalibraties uitvoerd! (Of eventueel de arm kalibratie elke keer!!)*/
00323             //if (metingstatus<5);
00324             //    state = ARMKALIBRATIE;
00325             //if (metingstatus==5);
00326             //    state = METEN_RICHTING;
00327             //break;
00328             //}
00329         }
00330 
00331         case ARM_KALIBRATIE: 
00332         {
00333             arm_kalibratie();
00334             if (current_herhalingen == 100) 
00335             {
00336                 current_herhalingen = 0;
00337                 previous_herhalingen = 0;
00338                 state = EMG_KALIBRATIE;
00339                 motor1.setPosition(0);
00340                 motor2.setPosition(0);
00341                 pwm_motor1.period_us(100);
00342                 pwm_motor2.period_us(100);
00343             }
00344             break;
00345         }
00346 
00347         case EMG_KALIBRATIE: 
00348         {
00349             emg_kalibratie();
00350             if (current_herhalingen >=1000) 
00351             {
00352                 state = METEN_HOOGTE;
00353                 current_herhalingen = 0;
00354                 previous_herhalingen = 0;
00355                 aantal_pieken = 0;
00356                 doel = 0;
00357             }
00358             break;
00359         }
00360 
00361         case METEN_HOOGTE: 
00362         {
00363             meten_hoogte();
00364             if (current_herhalingen == 2800 && aantal_pieken == 1) 
00365             {
00366                 state = METEN_RICHTING;
00367                 current_herhalingen = 0;
00368                 previous_herhalingen = 0;
00369                 aantal_pieken = 0;
00370                 doel = 0;
00371             }
00372             else
00373             {
00374                 state = METEN_HOOGTE;
00375                 current_herhalingen = 0;
00376                 previous_herhalingen = 0;
00377                 aantal_pieken = 0;
00378                 doel = 0;
00379             }
00380             break;
00381         }
00382 
00383         case METEN_RICHTING: 
00384         {
00385             meten_richting();
00386             if (current_herhalingen == 2800 && aantal_pieken == 1) 
00387             {
00388                 state = INSTELLEN_RICHTING;
00389                 current_herhalingen = 0;
00390                 previous_herhalingen = 0;
00391                 aantal_pieken = 0;
00392                 doel = 0;
00393             }
00394             else
00395             {
00396                 state = METEN_RICHTING;
00397                 current_herhalingen = 0;
00398                 previous_herhalingen = 0;
00399                 aantal_pieken = 0;
00400                 doel = 0;
00401             }
00402             break;
00403         }
00404 
00405         case INSTELLEN_RICHTING: 
00406         {
00407             instellen_richting();
00408             if (current_herhalingen == 100) 
00409             {
00410                 state = SLAAN;
00411                 current_herhalingen = 0;
00412                 previous_herhalingen = 0;
00413             }
00414         break; 
00415             
00416         }
00417 
00418         case SLAAN: 
00419         {
00420             GotoPosition(1.5 ,8);
00421             if (current_herhalingen == 100) 
00422             {
00423                 state = RETURN2RUST;
00424                 current_herhalingen = 0;
00425                 previous_herhalingen = 0;
00426                 prev_setpoint =0; 
00427                 setpoint =0;
00428                 
00429             }
00430         break;    
00431         }
00432 
00433         case RETURN2RUST: 
00434         {
00435             GotoPosition(0,2);
00436             if (current_herhalingen == 100) 
00437             {
00438                 state = RUST;
00439                 current_herhalingen = 0;
00440                 previous_herhalingen = 0;
00441             }
00442             
00443             break;
00444         }
00445         
00446         default: {
00447             state = RUST;
00448         }
00449 
00450     }//switch(state)
00451 }//void statemachinefunction
00452 
00453 
00454 void screenupdate(){
00455     if(state==RUST){
00456         lcd.cls(); 
00457         lcd.locate(0,0);
00458         lcd.printf("V.I.C.T.O.R.Y.");   //regel 1 LCD scherm
00459         lcd.locate(0,1);
00460         lcd.printf("  GROEP 7   ");
00461     }
00462     else if(state==EMG_KALIBRATIE){
00463         lcd.cls();
00464         lcd.locate(0,0);
00465         lcd.printf("Max. aanspannen");
00466         if(current_herhalingen<=200){
00467             lcd.locate(0,1); 
00468             lcd.printf("nog 5 sec.");
00469         }
00470         else if(current_herhalingen<=400){
00471             lcd.locate(0,1); 
00472             lcd.printf("nog 4 sec.");
00473         }
00474         else if(current_herhalingen<=600){
00475             lcd.locate(0,1); 
00476             lcd.printf("nog 3 sec.");
00477         }
00478         else if(current_herhalingen<=800){
00479             lcd.locate(0,1); 
00480             lcd.printf("nog 2 sec.");
00481         }
00482         else if(current_herhalingen<=1000){
00483             lcd.locate(0,1); 
00484             lcd.printf("nog 1 sec.");
00485         }
00486     }
00487     else if(state==METEN_HOOGTE){
00488         lcd.cls();
00489         if(current_herhalingen<=200){
00490             lcd.locate(0,0);
00491             lcd.printf("Hoogte bepalen:");
00492             lcd.locate(0,1);
00493             lcd.printf("span aan per vak");
00494         }
00495         else if(200<=current_herhalingen<=1200){
00496             lcd.locate(0,0);
00497             lcd.printf("Vak %d",doel);
00498             if(current_herhalingen<=400){
00499                 lcd.locate(0,1); 
00500                 lcd.printf("nog 5 sec.");
00501             }
00502             else if(current_herhalingen<=600){
00503                 lcd.locate(0,1); 
00504                 lcd.printf("nog 4 sec.");
00505             }
00506             else if(current_herhalingen<=800){
00507                 lcd.locate(0,1); 
00508                 lcd.printf("nog 3 sec.");
00509             }
00510             else if(current_herhalingen<=1000){
00511                 lcd.locate(0,1); 
00512                 lcd.printf("nog 2 sec.");
00513             }
00514             else if(current_herhalingen<=1200){
00515                 lcd.locate(0,1); 
00516                 lcd.printf("nog 1 sec.");
00517             }
00518         }
00519         else if(current_herhalingen<=1600){
00520             lcd.locate(0,0);
00521             lcd.printf("Vak %d akkoord?",doel);
00522             lcd.locate(0,1);
00523             lcd.printf("Span aan");
00524         }
00525         else if(1600<=current_herhalingen<=2800 && aantal_pieken==1){
00526             lcd.locate(0,0);
00527             lcd.printf("Vak %d akkoord",doel_hoogte);   
00528         }
00529         else{
00530             lcd.locate(0,0);
00531             lcd.printf("Opnieuw hoogte");
00532             lcd.locate(0,1);
00533             lcd.printf("bepalen");
00534         }
00535     }
00536     else if(state==METEN_RICHTING){
00537         lcd.cls();
00538         if(current_herhalingen<=200){
00539             lcd.locate(0,0);
00540             lcd.printf("Richting bepalen:");
00541             lcd.locate(0,1);
00542             lcd.printf("span aan per vak");
00543         }
00544         else if(200<=current_herhalingen<=1200){
00545             lcd.locate(0,0);
00546             lcd.printf("Vak %d",doel);
00547             if(current_herhalingen<=400){
00548                 lcd.locate(0,1); 
00549                 lcd.printf("nog 5 sec.");
00550             }
00551             else if(current_herhalingen<=600){
00552                 lcd.locate(0,1); 
00553                 lcd.printf("nog 4 sec.");
00554             }
00555             else if(current_herhalingen<=800){
00556                 lcd.locate(0,1); 
00557                 lcd.printf("nog 3 sec.");
00558             }
00559             else if(current_herhalingen<=1000){
00560                 lcd.locate(0,1); 
00561                 lcd.printf("nog 2 sec.");
00562             }
00563             else if(current_herhalingen<=1200){
00564                 lcd.locate(0,1); 
00565                 lcd.printf("nog 1 sec.");
00566             }
00567         }
00568         else if(current_herhalingen<=1600){
00569             lcd.locate(0,0);
00570             lcd.printf("Vak %d akkoord?",doel);
00571             lcd.locate(0,1);
00572             lcd.printf("Span aan");
00573         }
00574         else if(1600<=current_herhalingen<=2800 && aantal_pieken==1){
00575             lcd.locate(0,0);
00576             lcd.printf("Vak %d akkoord",doel_richting);   
00577         }
00578         else{
00579             lcd.locate(0,0);
00580             lcd.printf("Opnieuw richting");
00581             lcd.locate(0,1);
00582             lcd.printf("bepalen");
00583         }
00584     }
00585     else{     
00586         lcd.cls();
00587         lcd.printf("state %d", state); //hier nog aan toevoegen hoe je de 'waarde', dus eigenlijk tekst, die opgeslagen staat in state kan printen.
00588     }
00589 }
00590 
00591 int main(){
00592     pc.baud(115200);
00593     arm_biquad_cascade_df1_init_f32(&lowpass_1,1 , lowpass_1_const, lowpass_1_states);
00594     arm_biquad_cascade_df1_init_f32(&highpass,1 , highpass_const, highpass_states);
00595     arm_biquad_cascade_df1_init_f32(&notch,1 , notch_const, notch_states);
00596     arm_biquad_cascade_df1_init_f32(&lowpass_2,1 , lowpass_2_const, lowpass_2_states);
00597     statemachine.attach(&statemachinefunction, TSAMP); // the address of the function to be attached (flip) and the interval (2 seconds)
00598     screen.attach(&screenupdate, 0.2);
00599     while(1);
00600 }