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 
00022 /*
00023 #define vaste waarden
00024 */
00025 /*definieren pinnen Motor 1*/
00026 #define M1_PWM PTA5
00027 #define M1_DIR PTA4
00028 #define M2_PWM PTC8
00029 #define M2_DIR PTC9
00030 /*Definieren minimale waarden PWM per motor*/
00031 #define PWM1_min_50 0.529 /*met koppelstuk!*/
00032 #define PWM2_min_30 0.529 /*aanpassen! Klopt nog niet met koppelstuk*/
00033 /*Definieren om de hoeveel seconden er gegevens naar de HID-scope gestuurd worden.*/
00034 #define TSAMP 0.005
00035 #define K_P (5)
00036 #define K_I (0.1 * TSAMP)
00037 #define K_D (0)
00038 //#define K_D (0.0005 /TSAMP)
00039 #define I_LIMIT 100.
00040 #define PI 3.1415926535897
00041 #define lengte_arm 0.5
00042 
00043 /*
00044 Geef een naam aan een actie en vertel welke pinnen hiervoor gebruikt worden.
00045 */
00046 TextLCD lcd(PTD2, PTA12, PTB2, PTB3, PTC2, PTA13, TextLCD::LCD16x2); // rs, e, d4-d7
00047 Encoder motor1(PTD3,PTD1);
00048 Encoder motor2(PTD5, PTD0); 
00049 PwmOut pwm_motor1(M1_PWM);
00050 PwmOut pwm_motor2(M2_PWM); 
00051 DigitalOut motordir1(M1_DIR);
00052 DigitalOut motordir2(M2_DIR); 
00053 DigitalOut LEDGREEN(LED_GREEN);
00054 DigitalOut LEDRED(LED_RED); 
00055 Serial pc(USBTX,USBRX);
00056 /*
00057 definieer namen aan var, float, int, static float, uint8_t, uint16_t etc. en geef ze eventueel een waarde
00058 */
00059 Ticker statemachine;
00060 Ticker screen;
00061 int previous_herhalingen = 0;
00062 int current_herhalingen = 0;
00063 int PWM2_percentage = 100; 
00064 int aantal_rotaties_1 = 10;
00065 int aantalcr_1 = 1600;
00066 int aantalcr_2 = 960; 
00067 int beginpos_motor1;
00068 int beginpos_motor1_new; 
00069 int beginpos_motor2;
00070 int beginpos_motor2_new;
00071 int previous_pos_motor1 = 0;
00072 int current_pos_motor1;
00073 int EMG = 1;
00074 int delta_pos_motor1_puls;
00075 void clamp(float * in, float min, float max);
00076 volatile bool looptimerflag;
00077 int16_t gewenste_snelheid = 2;
00078 int16_t gewenste_snelheid_rad = 4; 
00079 int16_t gewenste_snelheid_rad_return = 1;
00080 float pid(float setpoint, float measurement);
00081 float pos_motor1_rad;
00082 float PWM1_percentage = 0;
00083 float delta_pos_motor1_rad;
00084 float snelheid_motor1_rad;
00085 float snelheid_arm_ms; 
00086 float PWM1; 
00087 float PWM2;
00088 float Speed_motor1;
00089 float Speed_motor1rad;
00090 float setpoint = 0;
00091 float prev_setpoint = 0; 
00092 
00093 //HIDScope scope(6);
00094 
00095 enum  state_t {RUST, ARM_KALIBRATIE, EMG_KALIBRATIE, METEN_RICHTING, METEN_HOOGTE, INSTELLEN_RICHTING, SLAAN, RETURN2RUST}; //verschillende stadia definieren voor gebruik in CASES
00096 state_t state = RUST;
00097 
00098 //functies die vanuit de statemachinefunction aangeroepen kunnen worden
00099 void rust() {
00100     current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
00101 }
00102     
00103 void arm_kalibratie() {
00104     //voor nu om de loop te doorlopen wordt onderstaande code gebruikt. Nogmaal gesproken moet er gewacht worden op een 'hoog' signaal van een knop
00105     current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
00106     motor1.setPosition(0);
00107     motor2.setPosition(0);    
00108 }
00109 
00110 void emg_kalibratie() {
00111     current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
00112 }
00113 
00114 void meten_richting() {
00115     current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
00116 }
00117 
00118 void meten_hoogte() {
00119     current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
00120 }
00121 
00122 void instellen_richting() {
00123     current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
00124 }
00125 
00126 void GotoPosition (float position_setpoint_rad, float speed_radpersecond) {
00127     
00128     current_pos_motor1 = motor1.getPosition(); //bekijk na elke 0.005s wat de huidige 'waarde' van de encoder is
00129     //delta_pos_motor1_puls = current_pos_motor1 - previous_pos_motor1; //Bereken wat het verschil in waarde is voor het berekenen van de snelheid(puls)
00130     pos_motor1_rad = current_pos_motor1/(1600/(2*PI)); //echte waarde omrekenen naar rad voor (positie) PID regelaar
00131     //delta_pos_motor1_rad = delta_pos_motor1_puls/(1600/(2*PI)); //delta waarde omrekenen naar rad voor snelheidsbepaling (rad)
00132     //snelheid_motor1_rad = delta_pos_motor1_rad / TSAMP; //rad delen door TSAMP om rad/s te verkrijgen
00133     //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
00134     //scope.set(0, snelheid_motor1_rad);
00135     
00136     previous_pos_motor1 = current_pos_motor1;  //sla de huidige waarde op als vorige waarde.
00137     
00138     //nu gaan we positie regelen i.p.v. snelheid.
00139     
00140     if (fabs(pos_motor1_rad - position_setpoint_rad) <= 0.05) //if position error < ...rad, then stop.
00141     {
00142         speed_radpersecond = 0; 
00143         setpoint = pos_motor1_rad;
00144         current_herhalingen = previous_herhalingen + 1; 
00145         previous_herhalingen = current_herhalingen;
00146         pc.printf("stop\n\r");
00147         PWM1_percentage = 0;
00148     }
00149     else if(pos_motor1_rad - position_setpoint_rad < 0)
00150     {    
00151         setpoint = prev_setpoint +( TSAMP * speed_radpersecond);
00152         PWM1_percentage = pid(setpoint, pos_motor1_rad);       
00153     }
00154     else
00155     {
00156         setpoint = prev_setpoint -( TSAMP * speed_radpersecond);
00157         PWM1_percentage = pid(setpoint, pos_motor1_rad);       
00158     }
00159     /*new_pwm = (setpoint - motor1.getPosition())*.001; -> P action*/
00160     pc.printf("%f\n\r",PWM1_percentage);
00161     
00162     
00163     //scope.set(1, setpoint-pos_motor1_rad); 
00164     
00165     if (PWM1_percentage < -100)
00166     {
00167         PWM1_percentage = -100;
00168     }
00169     else if (PWM1_percentage >100)
00170     {
00171         PWM1_percentage =100;
00172     }
00173     
00174     if(PWM1_percentage < 0)
00175     {
00176         motordir1 = 1;
00177     }
00178     else
00179     {
00180         motordir1 = 0;
00181     }
00182         
00183     pwm_motor1.write(abs(PWM1_percentage/100.));//abs(((1-PWM1_min_50)/100)*PWM1_percentage + PWM1_min_50));
00184     //scope.set(5, abs(((1-PWM1_min_50)/100)*PWM1_percentage + PWM1_min_50));
00185     prev_setpoint = setpoint;
00186     //scope.send();  
00187 }
00188 
00189 
00190 
00191 float pid(float setpoint, float measurement)
00192 {
00193     float error;
00194     static float prev_error = 0;
00195     float           out_p = 0;
00196     static float    out_i = 0;
00197     float           out_d = 0;
00198     error  = (setpoint-measurement);
00199     out_p  = error*K_P; 
00200     out_i += error*K_I;
00201     out_d  = (error-prev_error)*K_D;
00202     clamp(&out_i,-I_LIMIT,I_LIMIT);
00203     prev_error = error;
00204     //scope.set(2, out_p);
00205     //scope.set(3, out_i);
00206     //scope.set(4, out_d);
00207     return out_p + out_i + out_d;
00208 }
00209 
00210 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
00211 // maar de locatie van de variabele. 
00212 {
00213     *in > min ? /*(*/*in < max? /*niets doen*/ : *in = max/*)*/: *in = min; // a ? b : c --> als a waar is, dan doe je b, en anders c 
00214     // *in = het getal dat staat op locatie van in --> waarde van new_pwm
00215 }
00216     
00217 void statemachinefunction()
00218 {
00219     switch(state) {
00220         case RUST: {
00221             rust();
00222             /*voorwaarde wanneer hij door kan naar de volgende case*/
00223             if (current_herhalingen == 100 && EMG == 1) 
00224             {
00225                 current_herhalingen = 0;
00226                 previous_herhalingen = 0;
00227                 state = ARM_KALIBRATIE;
00228                 EMG = 0;
00229             }
00230             break;
00231             /*bepalen hoe je zorgt dat je maar 1x de kalibraties uitvoerd! (Of eventueel de arm kalibratie elke keer!!)*/
00232             //if (metingstatus<5);
00233             //    state = ARMKALIBRATIE;
00234             //if (metingstatus==5);
00235             //    state = METEN_RICHTING;
00236             //break;
00237             //}
00238         }
00239 
00240         case ARM_KALIBRATIE: 
00241         {
00242             arm_kalibratie();
00243             if (current_herhalingen == 100) 
00244             {
00245                 current_herhalingen = 0;
00246                 previous_herhalingen = 0;
00247                 state = EMG_KALIBRATIE;
00248                 motor1.setPosition(0);
00249                 motor2.setPosition(0);
00250                 pwm_motor1.period_us(100);
00251                 pwm_motor2.period_us(100);
00252             }
00253             break;
00254         }
00255 
00256         case EMG_KALIBRATIE: 
00257         {
00258             emg_kalibratie();
00259             if (current_herhalingen == 100) 
00260             {
00261                 state = METEN_RICHTING;
00262                 current_herhalingen = 0;
00263                 previous_herhalingen = 0;
00264             }
00265             break;
00266         }
00267 
00268         case METEN_RICHTING: 
00269         {
00270             meten_richting();
00271             if (current_herhalingen == 100) 
00272             {
00273                 state = METEN_HOOGTE;
00274                 current_herhalingen = 0;
00275                 previous_herhalingen = 0;
00276             }
00277             break;
00278         }
00279 
00280         case METEN_HOOGTE: 
00281         {
00282             meten_hoogte();
00283             if (current_herhalingen == 100) 
00284             {
00285                 state = INSTELLEN_RICHTING;
00286                 current_herhalingen = 0;
00287                 previous_herhalingen = 0;
00288             }
00289             break;
00290         }
00291 
00292         case INSTELLEN_RICHTING: 
00293         {
00294             instellen_richting();
00295             if (current_herhalingen == 100) 
00296             {
00297                 state = SLAAN;
00298                 current_herhalingen = 0;
00299                 previous_herhalingen = 0;
00300             }
00301         break; 
00302             
00303         }
00304 
00305         case SLAAN: 
00306         {
00307             GotoPosition(1.5 ,8);
00308             if (current_herhalingen == 100) 
00309             {
00310                 state = RETURN2RUST;
00311                 current_herhalingen = 0;
00312                 previous_herhalingen = 0;
00313                 prev_setpoint =0; 
00314                 setpoint =0;
00315                 
00316             }
00317         break;    
00318         }
00319 
00320         case RETURN2RUST: 
00321         {
00322             GotoPosition(0,2);
00323             if (current_herhalingen == 100) 
00324             {
00325                 state = RUST;
00326                 current_herhalingen = 0;
00327                 previous_herhalingen = 0;
00328             }
00329             
00330             break;
00331         }
00332         
00333         default: {
00334             state = RUST;
00335         }
00336 
00337     }//switch(state)
00338 }//void statemachinefunction
00339 
00340 
00341 void screenupdate(){
00342     if(state==RUST){
00343         lcd.cls(); 
00344         lcd.locate(0,0);
00345         lcd.printf("V.I.C.T.O.R.Y.");   //regel 1 LCD scherm
00346         lcd.locate(0,1);
00347         lcd.printf("  GROEP 7   ");
00348     }
00349     else{     
00350         lcd.cls();
00351         lcd.printf("state %d", state); //hier nog aan toevoegen hoe je de 'waarde', dus eigenlijk tekst, die opgeslagen staat in state kan printen.
00352     }
00353 }
00354 
00355 int main() {
00356     pc.baud(115200);
00357     statemachine.attach(&statemachinefunction, TSAMP); // the address of the function to be attached (flip) and the interval (2 seconds)
00358     screen.attach(&screenupdate, 0.2);
00359     while(1);
00360 }