main.cpp

00001 /* mbed Microcontroller Library
00002  * Copyright (c) 2019 ARM Limited
00003  * SPDX-License-Identifier: Apache-2.0
00004  */
00005 
00006 #include "mbed.h"
00007 #include "platform/mbed_thread.h"
00008 #include "Kanal.h"
00009 #include "eeprom_intern.h"
00010 #include "LCD.h"
00011 
00012 
00013 
00014 // Blinking rate in milliseconds
00015 #define BLINKING_RATE_MS                                                    50
00016 #define automatik 0
00017 DigitalIn TasteP(PA_1);
00018 DigitalIn TasteM(PA_6);
00019 DigitalIn TasteUE(PA_10);
00020 DigitalIn betriebsart(PB_7);
00021 PortIn achsauswahl(PortB,0b111);
00022 AnalogIn speed(PA_0);
00023 lcd mylcd;
00024 
00025 struct position
00026 {
00027     unsigned char w[6]; //6 Positonmen, Wertebereich: 0..180°, für 6 Achsen
00028     int ms;     //Fahrzeit in ms zur neuen Position
00029 };
00030 
00031 position schritt[10]; //Es werden 10 Positionen angefahren
00032 
00033 void toEEPROM(void)
00034 {
00035         EEPROM_WRITE(0, (char*)schritt, sizeof(schritt));
00036 }
00037 
00038 void fromEEPROM(void)
00039 {
00040     int zeiger=0;
00041         EEPROM_READ(0, (char*)schritt, sizeof(schritt));
00042 }
00043 
00044 int main()
00045 {
00046     int schrittnr=0,achs=0;
00047     betriebsart.mode(PullDown);
00048     achsauswahl.mode(PullDown);
00049     TasteP.mode(PullDown);
00050     TasteM.mode(PullDown);
00051     TasteUE.mode(PullDown);
00052     Kanal* achse[6];    //Der Roboter hat 6 Achsen:
00053     mylcd.clear();
00054     mylcd.cursorpos(0);
00055 
00056     for (int i=0;i<6;i++) achse[i]=new Kanal(i); //Die Achsobjekte werden erzeugt
00057     int dtc=0;
00058     // Initialise the digital pin LED1 as an output
00059     DigitalOut led(LED1);   //Diagnose LED auf dem Nukleo
00060     schritt[0].w[0]=150; //achse[0]: Greifer
00061     schritt[0].w[1]=90; //achse[1]: Drehachse für den Greifer
00062     schritt[0].w[2]=90; //achse[2]: Dritter Kipp-Servo, von unten her
00063     schritt[0].w[3]=90; //achse[3]: Zweiter Kipp-Servo, von unten her
00064     schritt[0].w[4]=90; //achse[4]: Erster Kipp-Servo auf der Drehachse 
00065     schritt[0].w[5]=90; //achse[5]: Grundachse, Drehung des Roboters
00066     schritt[0].ms=1000; //Fahrzeit
00067     
00068     schritt[1].w[0]=120;
00069     schritt[1].w[1]=180;
00070     schritt[1].w[2]=160;
00071     schritt[1].w[3]=15;
00072     schritt[1].w[4]=60;
00073     schritt[1].w[5]=70;
00074     schritt[1].ms=1000;
00075 
00076     schritt[2].w[0]=160;
00077     schritt[2].w[1]=180;
00078     schritt[2].w[2]=160;
00079     schritt[2].w[3]=15;
00080     schritt[2].w[4]=60;
00081     schritt[2].w[5]=70;
00082     schritt[2].ms=1000;
00083     
00084     schritt[3].w[0]=160;
00085     schritt[3].w[1]=180;
00086     schritt[3].w[2]=125;
00087     schritt[3].w[3]=45;
00088     schritt[3].w[4]=60;
00089     schritt[3].w[5]=70;
00090     schritt[3].ms=1000;
00091     
00092     schritt[4].w[0]=160;
00093     schritt[4].w[1]=180;
00094     schritt[4].w[2]=125;
00095     schritt[4].w[3]=45;
00096     schritt[4].w[4]=60;
00097     schritt[4].w[5]=150;
00098     schritt[4].ms=1000;
00099 
00100     schritt[5].w[0]=160;
00101     schritt[5].w[1]=180;
00102     schritt[5].w[2]=160;
00103     schritt[5].w[3]=15;
00104     schritt[5].w[4]=60;
00105     schritt[5].w[5]=150;
00106     schritt[5].ms=1000;
00107     
00108     schritt[6].w[0]=120;
00109     schritt[6].w[1]=180;
00110     schritt[6].w[2]=160;
00111     schritt[6].w[3]=15;
00112     schritt[6].w[4]=60;
00113     schritt[6].w[5]=150;
00114     schritt[6].ms=1000;
00115     schritt[7].ms=1000;
00116     schritt[8].ms=1000;
00117 
00118     //Grundstellung nch Start des Nucleo
00119     achse[0]->grundstellung(150);
00120     achse[1]->grundstellung(90);
00121     achse[2]->grundstellung(90);
00122     achse[3]->grundstellung(90);
00123     achse[4]->grundstellung(90);
00124     achse[5]->grundstellung(90);
00125     fromEEPROM();
00126     mylcd.cursorpos(0);
00127     mylcd.printf("Schritt=%d  ",schrittnr);
00128     mylcd.cursorpos(0x40);
00129     mylcd.printf("a: %d, w: %d  ",achs,schritt[schrittnr].w[achs]);
00130     while (true) {
00131         led = !led;
00132         if (betriebsart==automatik)
00133         {
00134             schrittnr=-1;
00135             for (int j=0;j<8;j++) //Schritte
00136             {
00137                 mylcd.cursorpos(0);
00138                 mylcd.printf("Schritt=%d  ",j);
00139                 //alle Achsen mit neuen Zielen versorgen und starten
00140                 for (int i=0;i<6;i++)   
00141                 {
00142                     //Operstion go(´float Zielwinkel, int fahrzeit
00143                     achse[i]->go(schritt[j].w[i],speed*10000+200);//schritt[j].ms); 
00144                 }
00145                 //warten, dass alle Achsen fertig werden
00146                 for (int i=0;i<6;i++)   
00147                 {
00148                     //Operation bool isBusy() 
00149                     //true=Achse bewegt sich noch, false=Achse hat Bewegung beendet
00150                     while(achse[i]->isBusy()); //-> gleichbedeutend mit .-Operator
00151                 }            
00152             }
00153         }
00154         else
00155         {
00156             achs=achsauswahl%6;
00157             if (schrittnr==-1)  //Init TeachIn
00158             {
00159                 schrittnr=0;
00160                 mylcd.cursorpos(0);
00161                 mylcd.printf("Schritt=%d  ",schrittnr);
00162                 mylcd.cursorpos(0x40);
00163                 mylcd.printf("a: %d, w: %d  ",achs,schritt[schrittnr].w[achs]);
00164                 for (int j=0;j<6;j++) achse[j]->go(schritt[schrittnr].w[j],1000);
00165                 for (int j=0;j<6;j++)   
00166                 {
00167                     //Operation bool isBusy() 
00168                     //true=Achse bewegt sich noch, false=Achse hat Bewegung beendet
00169                     while(achse[j]->isBusy()); //-> gleichbedeutend mit .-Operator
00170                 }                 
00171             }
00172             
00173             if (TasteP==1) 
00174             {
00175                 if (schritt[schrittnr].w[achs]<180) schritt[schrittnr].w[achs]++;
00176                 mylcd.cursorpos(0x40);
00177                 mylcd.printf("a: %d, w: %d  ",achs,schritt[schrittnr].w[achs]);
00178                 achse[achs]->go(schritt[schrittnr].w[achs],10);
00179 
00180             }
00181             if (TasteM==1) 
00182             {
00183                 if (schritt[schrittnr].w[achs]>0) schritt[schrittnr].w[achs]--;
00184                 mylcd.cursorpos(0x40);
00185                 mylcd.printf("a: %d, w: %d  ",achs,schritt[schrittnr].w[achs]);
00186                 achse[achs]->go(schritt[schrittnr].w[achs],10);
00187                 //for (int j=0;j<6;j++) achse[j]->go(schritt[schrittnr].w[j],25);
00188 
00189 
00190             }
00191             if (TasteUE==1)
00192             {
00193                 for (int i=0;i<10;i++) schritt[i].ms=1000;
00194                 schritt[0].ms=2000;
00195                 toEEPROM();
00196                 HAL_Delay(20);
00197                 
00198                 schrittnr=(schrittnr+1)%10;
00199                 mylcd.cursorpos(0);
00200                 mylcd.printf("Schritt=%d  ",schrittnr);
00201                 for (int j=0;j<6;j++) achse[j]->go(schritt[schrittnr].w[j],1000);
00202                 for (int j=0;j<6;j++)   
00203                 {
00204                     //Operation bool isBusy() 
00205                     //true=Achse bewegt sich noch, false=Achse hat Bewegung beendet
00206                     while(achse[j]->isBusy()); //-> gleichbedeutend mit .-Operator
00207                 }                 
00208                 while (TasteUE==1);
00209                 HAL_Delay(20);
00210             }
00211         }
00212         
00213     }
00214 }