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 (80000)
00036 #define K_I (0.01)
00037 #define K_D (0.01)
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 float marge = 0; 
00093 
00094 //HIDScope scope(6);
00095 
00096 enum  state_t {RUST, ARM_KALIBRATIE, EMG_KALIBRATIE, METEN_RICHTING, METEN_HOOGTE, INSTELLEN_RICHTING, SLAAN, RETURN2RUST}; //verschillende stadia definieren voor gebruik in CASES
00097 state_t state = RUST;
00098 
00099 //functies die vanuit de statemachinefunction aangeroepen kunnen worden
00100 void rust() {
00101     current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
00102 }
00103     
00104 void arm_kalibratie() {
00105     //voor nu om de loop te doorlopen wordt onderstaande code gebruikt. Nogmaal gesproken moet er gewacht worden op een 'hoog' signaal van een knop
00106     current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
00107     motor1.setPosition(0);
00108     motor2.setPosition(0);    
00109 }
00110 
00111 void emg_kalibratie() {
00112     current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
00113 }
00114 
00115 void meten_richting() {
00116     current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
00117 }
00118 
00119 void meten_hoogte() {
00120     current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
00121 }
00122 
00123 void instellen_richting() {
00124     current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
00125 }
00126 
00127 void GotoPosition (float position_setpoint_rad, float speed_radpersecond, float marge) {
00128     
00129     current_pos_motor1 = motor1.getPosition(); //bekijk na elke 0.005s wat de huidige 'waarde' van de encoder is
00130     //delta_pos_motor1_puls = current_pos_motor1 - previous_pos_motor1; //Bereken wat het verschil in waarde is voor het berekenen van de snelheid(puls)
00131     pos_motor1_rad = current_pos_motor1/(1600/(2*PI)); //echte waarde omrekenen naar rad voor (positie) PID regelaar
00132     //delta_pos_motor1_rad = delta_pos_motor1_puls/(1600/(2*PI)); //delta waarde omrekenen naar rad voor snelheidsbepaling (rad)
00133     //snelheid_motor1_rad = delta_pos_motor1_rad / TSAMP; //rad delen door TSAMP om rad/s te verkrijgen
00134     //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
00135     //scope.set(0, snelheid_motor1_rad);
00136     
00137     previous_pos_motor1 = current_pos_motor1;  //sla de huidige waarde op als vorige waarde.
00138     
00139     //nu gaan we positie regelen i.p.v. snelheid.
00140     
00141     if (fabs(pos_motor1_rad - position_setpoint_rad) <= marge) //if position error < ...rad, then stop.
00142     {
00143         speed_radpersecond = 0; 
00144         setpoint = pos_motor1_rad;
00145         current_herhalingen = previous_herhalingen + 1; 
00146         previous_herhalingen = current_herhalingen;
00147         pc.printf("stop\n\r");
00148         PWM1_percentage = 0;
00149     }
00150     else if(pos_motor1_rad - position_setpoint_rad < 0)
00151     {    
00152         setpoint = prev_setpoint +( TSAMP * speed_radpersecond);
00153         PWM1_percentage = pid(setpoint, pos_motor1_rad);       
00154     }
00155     else
00156     {
00157         setpoint = prev_setpoint -( TSAMP * speed_radpersecond);
00158         PWM1_percentage = pid(setpoint, pos_motor1_rad);       
00159     }
00160     /*new_pwm = (setpoint - motor1.getPosition())*.001; -> P action*/
00161     pc.printf("%d\n\r",current_pos_motor1);
00162     
00163     
00164     //scope.set(1, setpoint-pos_motor1_rad); 
00165     
00166     if (PWM1_percentage < -100)
00167     {
00168         PWM1_percentage = -100;
00169     }
00170     else if (PWM1_percentage >100)
00171     {
00172         PWM1_percentage =100;
00173     }
00174     
00175     if(PWM1_percentage < 0)
00176     {
00177         motordir1 = 1;
00178     }
00179     else
00180     {
00181         motordir1 = 0;
00182     }
00183         
00184     pwm_motor1.write(abs(PWM1_percentage/100.));//abs(((1-PWM1_min_50)/100)*PWM1_percentage + PWM1_min_50));
00185     //scope.set(5, abs(((1-PWM1_min_50)/100)*PWM1_percentage + PWM1_min_50));
00186     prev_setpoint = setpoint;
00187     //scope.send();  
00188 }
00189 
00190 
00191 
00192 float pid(float setpoint, float measurement)
00193 {
00194     float error;
00195     static float prev_error = 0;
00196     float           out_p = 0;
00197     static float    out_i = 0;
00198     float           out_d = 0;
00199     error  = (setpoint-measurement);
00200     out_p  = error*K_P; 
00201     out_i += error*K_I;
00202     out_d  = (error-prev_error)*K_D;
00203     clamp(&out_i,-I_LIMIT,I_LIMIT);
00204     prev_error = error;
00205     //scope.set(2, out_p);
00206     //scope.set(3, out_i);
00207     //scope.set(4, out_d);
00208     return out_p + out_i + out_d;
00209 }
00210 
00211 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
00212 // maar de locatie van de variabele. 
00213 {
00214     *in > min ? /*(*/*in < max? /*niets doen*/ : *in = max/*)*/: *in = min; // a ? b : c --> als a waar is, dan doe je b, en anders c 
00215     // *in = het getal dat staat op locatie van in --> waarde van new_pwm
00216 }
00217     
00218 void statemachinefunction()
00219 {
00220     switch(state) {
00221         case RUST: {
00222             rust();
00223             /*voorwaarde wanneer hij door kan naar de volgende case*/
00224             if (current_herhalingen == 100 && EMG == 1) 
00225             {
00226                 current_herhalingen = 0;
00227                 previous_herhalingen = 0;
00228                 state = ARM_KALIBRATIE;
00229                 EMG = 0;
00230             }
00231             break;
00232             /*bepalen hoe je zorgt dat je maar 1x de kalibraties uitvoerd! (Of eventueel de arm kalibratie elke keer!!)*/
00233             //if (metingstatus<5);
00234             //    state = ARMKALIBRATIE;
00235             //if (metingstatus==5);
00236             //    state = METEN_RICHTING;
00237             //break;
00238             //}
00239         }
00240 
00241         case ARM_KALIBRATIE: 
00242         {
00243             arm_kalibratie();
00244             if (current_herhalingen == 100) 
00245             {
00246                 current_herhalingen = 0;
00247                 previous_herhalingen = 0;
00248                 state = EMG_KALIBRATIE;
00249                 motor1.setPosition(0);
00250                 motor2.setPosition(0);
00251                 pwm_motor1.period_us(100);
00252                 pwm_motor2.period_us(100);
00253             }
00254             break;
00255         }
00256 
00257         case EMG_KALIBRATIE: 
00258         {
00259             emg_kalibratie();
00260             if (current_herhalingen == 100) 
00261             {
00262                 state = METEN_RICHTING;
00263                 current_herhalingen = 0;
00264                 previous_herhalingen = 0;
00265             }
00266             break;
00267         }
00268 
00269         case METEN_RICHTING: 
00270         {
00271             meten_richting();
00272             if (current_herhalingen == 100) 
00273             {
00274                 state = METEN_HOOGTE;
00275                 current_herhalingen = 0;
00276                 previous_herhalingen = 0;
00277             }
00278             break;
00279         }
00280 
00281         case METEN_HOOGTE: 
00282         {
00283             meten_hoogte();
00284             if (current_herhalingen == 100) 
00285             {
00286                 state = INSTELLEN_RICHTING;
00287                 current_herhalingen = 0;
00288                 previous_herhalingen = 0;
00289             }
00290             break;
00291         }
00292 
00293         case INSTELLEN_RICHTING: 
00294         {
00295             instellen_richting();
00296             if (current_herhalingen == 100) 
00297             {
00298                 state = RETURN2RUST;
00299                 current_herhalingen = 0;
00300                 previous_herhalingen = 0;
00301             }
00302             break; 
00303             
00304         }
00305 
00306         case SLAAN: 
00307         {
00308             GotoPosition(1.5 ,8, 0.1);
00309             if (current_herhalingen == 100) 
00310             {
00311                 state = RETURN2RUST;
00312                 current_herhalingen = 0;
00313                 previous_herhalingen = 0;
00314                 prev_setpoint =0; 
00315                 setpoint =0;
00316                 
00317             }
00318         break;    
00319         }
00320 
00321         case RETURN2RUST: 
00322         {
00323             GotoPosition(0,4, 0.05);
00324             //if (current_herhalingen == 100) 
00325             //{
00326             //    state = RUST;
00327             //    current_herhalingen = 0;
00328             //    previous_herhalingen = 0;
00329             //}
00330             
00331             break;
00332         }
00333         
00334         default: {
00335             state = RUST;
00336         }
00337 
00338     }//switch(state)
00339 }//void statemachinefunction
00340 
00341 
00342 void screenupdate(){
00343     if(state==RUST){
00344         lcd.cls(); 
00345         lcd.locate(0,0);
00346         lcd.printf("V.I.C.T.O.R.Y.");   //regel 1 LCD scherm
00347         lcd.locate(0,1);
00348         lcd.printf("  GROEP 7   ");
00349     }
00350     else{     
00351         lcd.cls();
00352         lcd.printf("state %d", state); //hier nog aan toevoegen hoe je de 'waarde', dus eigenlijk tekst, die opgeslagen staat in state kan printen.
00353     }
00354 }
00355 
00356 int main() {
00357     pc.baud(115200);
00358     statemachine.attach(&statemachinefunction, TSAMP); // the address of the function to be attached (flip) and the interval (2 seconds)
00359     screen.attach(&screenupdate, 0.2);
00360     while(1);
00361 }