API tools faq
paste
Guest User

Untitled

a guest
Apr 24th, 2018
69
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C 21.96 KB | None | 0 0
raw download clone embed print report
  1. /***************************************************/
  2. /* Programmname: PROJEKT func.h                    */
  3. /***************************************************/
  4. /* Programm: Grafische Darstellung und Simulation  */
  5. /*           eines Roboterarms                     */
  6. /*                                                 */
  7. /* Copyright: sasi
  8. /*                                                 */
  9. /* Datum: 30.01.2006                               */
  10. /*                                                 */
  11. /***************************************************/
  12.  
  13. //Bool erstellen (Für boolean datentype)
  14. enum bool {false, true};
  15.  
  16. //Struktur von ZeichenFesnter (ViewPORTS)
  17. struct viewPortStruct
  18. {
  19.     int left;
  20.     int top;
  21.     int right;
  22.     int bottom;
  23.     int clip;
  24.     int hintergrund;
  25.     int NullPunktVerschiebungX;
  26. } vPort1,vPort2,vPort3; //Direkte Deklaration
  27.  
  28. //Struktur für Roboterarm Informationen
  29. struct robo_arm
  30. {
  31.     double x1;
  32.     double y1;
  33.     double x2;
  34.     double y2;
  35.     double radius;
  36.     char text[100];
  37. }ober_arm, unter_arm;
  38.  
  39. //Struktur von Koordinaten system
  40. struct Koordinatensystem
  41. {
  42.     float faktor;
  43.     float x1;
  44.     float x2;
  45.     float y1;
  46.     float y2;
  47.     int farbe;
  48.     float alfa;
  49.     float beta;
  50.     float alfa_alt;
  51.     float beta_alt;
  52. } alpha_beta;
  53.  
  54. //Struktur für Weltkoordinate
  55. struct weltStruct
  56. {
  57.     float xmin;
  58.     float xmax;
  59.     float ymin;
  60.     float ymax;
  61. } welt1, welt2 ,welt3;
  62.  
  63. //Struktur fuer eingaben
  64. struct eingabefeld
  65. {
  66.     char ex1[50];
  67.     char ex2[50];
  68.     char ey1[50];
  69.     char ey2[50];
  70.     char modi[10];
  71. } Input;
  72.  
  73. //In Grafikmodus Umschalten
  74. void Load_BGI_Driver(char Treiberpfad[])
  75. {
  76.     int Grafiktreiber, Grafikmodus;
  77.  
  78.     //Grafiktreiber bestimmen und Laden
  79.     Grafiktreiber = DETECT;
  80.     //Grafik initialiseren
  81.     initgraph(&Grafiktreiber,&Grafikmodus,Treiberpfad);
  82.  
  83.     //Falls Fehler auftreten Programm mit Fehlermeldung beenden
  84.     if(Grafiktreiber<=0)
  85.     {
  86.         cputs(grapherrormsg(Grafiktreiber));
  87.         exit(1);
  88.     }
  89. }
  90.  
  91. //Zeigt die Umrisse des Übergebenen Viewports
  92. void VpShow(viewPortStruct viewPort)
  93. {
  94.     setviewport(viewPort.left,
  95.         viewPort.top,
  96.         viewPort.right,
  97.         viewPort.bottom,
  98.         viewPort.clip);
  99.  
  100.     rectangle(0,0,
  101.         (viewPort.right-viewPort.left),
  102.         (viewPort.bottom-viewPort.top));
  103.    
  104.     //Füllt den raum mit hintergrundFarbe
  105.     setfillstyle(1, viewPort.hintergrund);
  106.  
  107.     bar3d(0, 0,
  108.         (viewPort.right-viewPort.left),
  109.         (viewPort.bottom-viewPort.top), 0, 0);
  110. }
  111.  
  112. //Zeichnet einen farbigen Punkt
  113. void VpPunkt(int x, int y, int farbe,viewPortStruct viewPort)
  114. {
  115.     setviewport(viewPort.left,
  116.         viewPort.top,
  117.         viewPort.right,
  118.         viewPort.bottom,
  119.         viewPort.clip);
  120.  
  121.     putpixel(x,
  122.         (viewPort.bottom-viewPort.top)-y,
  123.         farbe);
  124. }
  125.  
  126. //Zeichnet eine linie
  127. void VpLinie(int x1, int y1, int x2, int y2,viewPortStruct viewPort)
  128. {
  129.     setviewport(viewPort.left,
  130.         viewPort.top,
  131.         viewPort.right,
  132.         viewPort.bottom,
  133.         viewPort.clip);
  134.  
  135.     line(x1,
  136.         (viewPort.bottom-viewPort.top)-y1,
  137.         x2,
  138.         (viewPort.bottom-viewPort.top)-y2);
  139. }
  140.  
  141. //Löscht den Inhalt eines ViewPorts
  142. void VpReset(viewPortStruct viewPort)
  143. {
  144.     setviewport(viewPort.left,
  145.         viewPort.top,
  146.         viewPort.right,
  147.         viewPort.bottom,
  148.         viewPort.clip);
  149.  
  150.     clearviewport();
  151. }
  152.  
  153. //Gezielte Text ausgabe
  154. void VpText(int x, int y, char Text[], viewPortStruct viewPort)
  155. {
  156.     setviewport(viewPort.left,
  157.         viewPort.top,
  158.         viewPort.right,
  159.         viewPort.bottom,
  160.         viewPort.clip);
  161.  
  162.     outtextxy(x,
  163.         (viewPort.bottom-viewPort.top)-y,
  164.         Text);
  165. }
  166.  
  167. //Welkoordinate in gerätekoordinate umwandeln
  168. int XWeltToViewPort(float xW, weltStruct welt, viewPortStruct viewPort)
  169. {
  170.     double delta_xG, delta_xW, xG;
  171.     int dummy;
  172.  
  173.     delta_xG=viewPort.right-viewPort.left;
  174.     delta_xW=welt.xmax-welt.xmin;
  175.  
  176.     //xG=viewPort.left+((delta_xG/delta_xW)*(xW-welt.xmin));
  177.     xG=((delta_xG/delta_xW)*(xW-welt.xmin));
  178.  
  179.     dummy = xG/1;
  180.     if ((xG-0.5)<dummy)
  181.     {
  182.         dummy=floor(xG);
  183.     }
  184.     else
  185.     {
  186.         dummy=ceil(xG);
  187.     }
  188.  
  189.     return dummy;
  190. }
  191.  
  192. int YWeltToViewPort(float yW, weltStruct welt, viewPortStruct viewPort)
  193. {
  194.     double delta_yG, delta_yW, yG;
  195.     int dummy;
  196.  
  197.     delta_yG=(viewPort.bottom-viewPort.top);
  198.     delta_yW=welt.ymax-welt.ymin;
  199.  
  200.     //yG=viewPort.top+((delta_yG/delta_yW)*(yW-welt.ymin));
  201.     yG=((delta_yG/delta_yW)*(yW-welt.ymin));
  202.  
  203.     dummy = yG/1;
  204.     if ((yG-0.5)<dummy)
  205.     {
  206.         dummy=floor(yG);
  207.     }
  208.     else
  209.     {
  210.         dummy=ceil(yG);
  211.     }
  212.  
  213.     return dummy;
  214. }
  215.  
  216. //Viewportszeichnen
  217. void InitViewPort1(void)
  218. {
  219.     //Viewport1 erstellen
  220.     vPort1.left = 10;
  221.     vPort1.top = 10;
  222.     vPort1.right = getmaxx()/2;
  223.     vPort1.bottom = (getmaxy()*0.7);
  224.     vPort1.clip = 1;
  225.     vPort1.hintergrund = 9;
  226.     vPort1.NullPunktVerschiebungX = vPort1.left;
  227.     VpShow(vPort1);
  228.     //Ende Viewport1
  229. }
  230. void InitViewPort2(void)
  231. {
  232.     //ANfang Viewport2
  233.     vPort2.left = vPort1.right+10;
  234.     vPort2.top = vPort1.top;
  235.     vPort2.right = getmaxx()-vPort1.left;
  236.     vPort2.bottom = vPort1.bottom;
  237.     vPort2.clip=1;
  238.     vPort2.hintergrund = 9;
  239.     vPort2.NullPunktVerschiebungX = vPort2.left;
  240.     VpShow(vPort2);
  241.     //Viewport2 ende
  242. }
  243. void InitViewPort3(void)
  244. {
  245.     //ANfang Viewport3
  246.     vPort3.left = vPort1.left;
  247.     vPort3.top = vPort2.bottom+10;
  248.     vPort3.right = vPort2.right;
  249.     vPort3.bottom = getmaxy()-(vPort3.top-vPort2.bottom);
  250.     vPort3.clip=1;
  251.     vPort3.hintergrund = 9;
  252.     vPort3.NullPunktVerschiebungX = vPort3.left;
  253.     VpShow(vPort3);
  254.     //Viewport3 ende
  255. }
  256.  
  257. // Koordinate initialisieren
  258. void kord_achsen(void)
  259. {
  260.     char Beschriftung[4];
  261.  
  262.     //Koordinaten Achsen
  263.     VpLinie(XWeltToViewPort(alpha_beta.x1,welt2,vPort2),
  264.         YWeltToViewPort(alpha_beta.y1,welt2,vPort2),
  265.         XWeltToViewPort(alpha_beta.x2,welt2,vPort2),
  266.         YWeltToViewPort(alpha_beta.y1,welt2,vPort2),
  267.         vPort2);
  268.  
  269.     VpLinie(XWeltToViewPort(alpha_beta.x1,welt2,vPort2),
  270.         YWeltToViewPort(alpha_beta.y1,welt2,vPort2),
  271.         XWeltToViewPort(alpha_beta.x1,welt2,vPort2),
  272.         YWeltToViewPort(alpha_beta.y2,welt2,vPort2),
  273.         vPort2);
  274.  
  275.     //Achsen beschriftung
  276.     VpText(XWeltToViewPort(alpha_beta.x2+3,welt2,vPort2),
  277.         YWeltToViewPort(alpha_beta.y1+3,welt2,vPort2),
  278.         "Alpha",
  279.         vPort2);
  280.  
  281.     VpText(XWeltToViewPort(alpha_beta.x1-13,welt2,vPort2),
  282.          YWeltToViewPort(alpha_beta.y2+13,welt2,vPort2),
  283.          "Beta",
  284.          vPort2);
  285.  
  286.     //Überschrift
  287.     VpText(XWeltToViewPort(alpha_beta.x1+40,welt2,vPort2),
  288.         YWeltToViewPort(alpha_beta.y2+40,welt2,vPort2),
  289.         "0 < Alpha < 180",
  290.         vPort2);
  291.  
  292.     VpText(XWeltToViewPort(alpha_beta.x1+40,welt2,vPort2),
  293.         YWeltToViewPort(alpha_beta.y2+30,welt2,vPort2),
  294.         "0 < Beta  < 180",
  295.         vPort2);
  296.  
  297.     //skalierung
  298.     for (int i=0; i<=(alpha_beta.x2-alpha_beta.x1);i+=30)
  299.     {
  300.         VpLinie(XWeltToViewPort(alpha_beta.x1+i,welt2,vPort2),
  301.             YWeltToViewPort(alpha_beta.y1,welt2,vPort2),
  302.             XWeltToViewPort(alpha_beta.x1+i,welt2,vPort2),
  303.             YWeltToViewPort((alpha_beta.y1+alpha_beta.faktor),welt2,vPort2),
  304.             vPort2);
  305.  
  306.         if(i%30==0 || i==0)
  307.         {
  308.             sprintf(Beschriftung,"%d",i);
  309.  
  310.             VpText(XWeltToViewPort((alpha_beta.x1+i)-8,welt2,vPort2),
  311.                 YWeltToViewPort((alpha_beta.y1+alpha_beta.faktor)-5,welt2,vPort2),
  312.                 Beschriftung,
  313.                 vPort2);
  314.         }
  315.     }
  316.  
  317.     for (i=0; i<=alpha_beta.x2-alpha_beta.x1;i+=30)
  318.     {
  319.         VpLinie(XWeltToViewPort(alpha_beta.x1,welt2,vPort2),
  320.             YWeltToViewPort(alpha_beta.y1+i,welt2,vPort2),
  321.             XWeltToViewPort((alpha_beta.x1+alpha_beta.faktor),welt2,vPort2),
  322.             YWeltToViewPort(alpha_beta.y1+i,welt2,vPort2),
  323.             vPort2);
  324.  
  325.         if(i%30==0 || i==0)
  326.         {
  327.             sprintf(Beschriftung,"%d",i);
  328.  
  329.             VpText(XWeltToViewPort((alpha_beta.x1+alpha_beta.faktor)-22,welt2,vPort2),
  330.                 YWeltToViewPort((alpha_beta.y1+i)+3,welt2,vPort2),
  331.                 Beschriftung,
  332.                 vPort2);
  333.         }
  334.     }
  335. }
  336.  
  337. //Koordinaten kreuz zeichnen abhängig vom ALFA BETA
  338. void VpKoordinatenSystem(Koordinatensystem alpha_beta, viewPortStruct viewPort)
  339. {
  340.     int col=getcolor();
  341.     setcolor(alpha_beta.farbe);
  342.  
  343.     VpLinie(XWeltToViewPort((alpha_beta.x1+alpha_beta.alfa_alt),welt2,viewPort),
  344.         YWeltToViewPort((alpha_beta.y1+alpha_beta.beta_alt),welt2,viewPort),
  345.         XWeltToViewPort((alpha_beta.x1+alpha_beta.alfa),welt2,viewPort),
  346.         YWeltToViewPort((alpha_beta.y1+alpha_beta.beta),welt2,viewPort),
  347.         viewPort);
  348.  
  349.     setcolor(col);
  350. }
  351.  
  352. //ROBOTER ARM ZEICHNEN
  353. bool ZeichneRobot(float alfa, float beta, viewPortStruct viewPort)
  354. {
  355.     if ((beta>=0) && (beta<=180) && (alfa>=0) && (alfa<=180))
  356.     {  
  357.         //Alte A B postionen merken
  358.         alpha_beta.alfa_alt = alpha_beta.alfa;
  359.         alpha_beta.beta_alt = alpha_beta.beta;
  360.  
  361.         alpha_beta.alfa = alfa;
  362.         alpha_beta.beta = beta;
  363.  
  364.         VpKoordinatenSystem(alpha_beta, vPort2);
  365.  
  366.         int col=getcolor();
  367.         setcolor(viewPort.hintergrund);
  368.  
  369.         //meldung löschen
  370.         VpText(XWeltToViewPort(ober_arm.x1-50,welt1,viewPort),
  371.             YWeltToViewPort(ober_arm.y1-50,welt1,viewPort),
  372.             ober_arm.text,
  373.             viewPort);
  374.  
  375.         //zeichne oberarm
  376.         VpLinie(XWeltToViewPort(ober_arm.x1,welt1,viewPort),
  377.             YWeltToViewPort(ober_arm.y1,welt1,viewPort),
  378.             XWeltToViewPort(ober_arm.x2,welt1,viewPort),
  379.             YWeltToViewPort(ober_arm.y2,welt1,viewPort),
  380.             viewPort);
  381.  
  382.         //zeichner unterarm
  383.         VpLinie(XWeltToViewPort(unter_arm.x1,welt1,viewPort),
  384.             YWeltToViewPort(unter_arm.y1,welt1,viewPort),
  385.             XWeltToViewPort(unter_arm.x2,welt1,viewPort),
  386.             YWeltToViewPort(unter_arm.y2,welt1,viewPort),
  387.             viewPort);
  388.    
  389.         //rechnet zum beta alfa dazu wenn beta konstat ist
  390.         //und alfa sich ändert, muss beta so bleiben wie er war
  391.  
  392.         beta=alfa+beta;
  393.  
  394.         //beta=alfa-beta;
  395.  
  396.         ober_arm.x2 = ober_arm.x1+(ober_arm.radius*cos((alfa*M_PI)/(double)180.0));
  397.         ober_arm.y2 = ober_arm.y1+(ober_arm.radius*sin((alfa*M_PI)/(double)180.0));
  398.         unter_arm.x1 = ober_arm.x2;
  399.         unter_arm.y1 = ober_arm.y2;
  400.         unter_arm.x2 = ober_arm.x2-(unter_arm.radius*cos((beta*M_PI)/(double)180.0));
  401.         unter_arm.y2 = ober_arm.y2-(unter_arm.radius*sin((beta*M_PI)/(double)180.0));
  402.  
  403.         setcolor(col);
  404.  
  405.         //zeichne oberarm
  406.         VpLinie(XWeltToViewPort(ober_arm.x1,welt1,viewPort),
  407.             YWeltToViewPort(ober_arm.y1,welt1,viewPort),
  408.             XWeltToViewPort(ober_arm.x2,welt1,viewPort),
  409.             YWeltToViewPort(ober_arm.y2,welt1,viewPort),
  410.             viewPort);
  411.  
  412.         //zeichner unterarm
  413.         VpLinie(XWeltToViewPort(unter_arm.x1,welt1,viewPort),
  414.             YWeltToViewPort(unter_arm.y1,welt1,viewPort),
  415.             XWeltToViewPort(unter_arm.x2,welt1,viewPort),
  416.             YWeltToViewPort(unter_arm.y2,welt1,viewPort),
  417.             viewPort);
  418.         return true;
  419.  
  420.     }
  421.     return false;
  422. }
  423.  
  424. //Bewegt den RoboterArm Linear
  425. void BewegeLienar(float alfa1, float beta1, float alfa2, float beta2,viewPortStruct viewPort)
  426. {
  427.     float delta_alfa, delta_beta;
  428.  
  429.     delta_alfa = alfa1 - alpha_beta.alfa;
  430.     delta_beta = beta1 - alpha_beta.beta;
  431.  
  432.     if(alfa1>alpha_beta.alfa)
  433.     {
  434.         for(int d=alpha_beta.alfa; d<=alfa1; d++)
  435.         {
  436.             ZeichneRobot(d,(alpha_beta.beta+(delta_beta/delta_alfa)),viewPort);
  437.             delay(50);
  438.         }
  439.     }
  440.     else
  441.     {
  442.         for(int d=alpha_beta.alfa; d>=alfa1; d--)
  443.         {
  444.             ZeichneRobot(d,(alpha_beta.beta-(delta_beta/delta_alfa)),viewPort);
  445.             delay(50);
  446.         }
  447.     }
  448.  
  449.     delay(500);
  450.  
  451.     delta_alfa = alfa2 - alpha_beta.alfa;
  452.     delta_beta = beta2 - alpha_beta.beta;
  453.  
  454.     if(alfa2>alpha_beta.alfa)
  455.     {
  456.         for(int d=alpha_beta.alfa; d<=alfa2; d++)
  457.         {
  458.             ZeichneRobot(d,(alpha_beta.beta+(delta_beta/delta_alfa)),viewPort);
  459.             delay(50);
  460.         }
  461.     }
  462.     else
  463.     {
  464.         for(int d=alpha_beta.alfa; d>=alfa2; d--)
  465.         {
  466.             ZeichneRobot(d,(alpha_beta.beta-(delta_beta/delta_alfa)),viewPort);
  467.             delay(50);
  468.         }
  469.     }
  470. }
  471.  
  472. //bewegt das arm sequentiell
  473. void BewegeSequent(float alfa1, float beta1, float alfa2, float beta2,viewPortStruct viewPort)
  474. {
  475.     //Sequentielle Bewegung
  476.     if(alfa1>alpha_beta.alfa)
  477.     {
  478.         for(int i=alpha_beta.alfa; i<=alfa1; i++)
  479.         {
  480.             ZeichneRobot(i,alpha_beta.beta,viewPort);
  481.             delay(50);
  482.         }
  483.     }
  484.     else
  485.     {
  486.         for(int i=alpha_beta.alfa; i>=alfa1; i--)
  487.         {
  488.             ZeichneRobot(i,alpha_beta.beta,viewPort);
  489.             delay(50);
  490.         }
  491.     }
  492.  
  493.     if(beta1>alpha_beta.beta)
  494.     {
  495.         for(int i=alpha_beta.beta; i<=beta1; i++)
  496.         {
  497.             ZeichneRobot(alpha_beta.alfa,i,viewPort);
  498.             delay(50);
  499.         }
  500.     }
  501.     else
  502.     {
  503.         for(int i=alpha_beta.beta; i>=beta1; i--)
  504.         {
  505.             ZeichneRobot(alpha_beta.alfa,i,viewPort);
  506.             delay(50);
  507.         }
  508.     }
  509.  
  510.     delay(500);
  511.    
  512.     if(alfa2>alpha_beta.alfa)
  513.     {
  514.         for(int i=alpha_beta.alfa; i<=alfa2; i++)
  515.         {
  516.             ZeichneRobot(i,alpha_beta.beta,viewPort);
  517.             delay(50);
  518.         }
  519.     }
  520.     else
  521.     {
  522.         for(int i=alpha_beta.alfa; i>=alfa2; i--)
  523.         {
  524.             ZeichneRobot(i,alpha_beta.beta,viewPort);
  525.             delay(50);
  526.         }
  527.     }
  528.  
  529.     if(beta2>alpha_beta.beta)
  530.     {
  531.         for(int i=alpha_beta.beta; i<=beta2; i++)
  532.         {
  533.             ZeichneRobot(alpha_beta.alfa,i,viewPort);
  534.             delay(50);
  535.         }
  536.     }
  537.     else
  538.     {
  539.         for(int i=alpha_beta.beta; i>=beta2; i--)
  540.         {
  541.             ZeichneRobot(alpha_beta.alfa,i,viewPort);
  542.             delay(50);
  543.         }
  544.     }
  545. }
  546.  
  547. //ermittelt aus x und y Wert den alfa winkel
  548. float GetRobotAlfa(float xWelt, float yWelt)
  549. {
  550.     double a,b,c;
  551.     float alfa, hilfe;
  552.  
  553.     //seitenlänge
  554.     a=sqrt(((xWelt-ober_arm.x1)*(xWelt-ober_arm.x1))+((yWelt-ober_arm.y1)*(yWelt-ober_arm.y1)));
  555.     b=unter_arm.radius;
  556.     c=ober_arm.radius;
  557.  
  558.     //AlfaWinkel
  559.     alfa=((c*c)+(a*a)-(b*b));
  560.     alfa=alfa/(2*c*a);
  561.  
  562.     if(alfa!=0)
  563.     {
  564.         alfa=(acos(alfa)*180)/M_PI;
  565.     }
  566.     else
  567.     {
  568.         alfa=acos(alfa);
  569.     }
  570.  
  571.     hilfe=(((xWelt-ober_arm.x1)*(xWelt-ober_arm.x1))+(a*a)-((yWelt-ober_arm.y1)*(yWelt-ober_arm.y1)));
  572.    
  573.     if(hilfe!=0)
  574.     {
  575.         hilfe=hilfe/(2*(xWelt-ober_arm.x1)*a);
  576.         hilfe=(acos(hilfe)*180)/M_PI;
  577.     }
  578.     else
  579.     {
  580.         hilfe=90;
  581.     }
  582.  
  583.     if(yWelt<0)
  584.     {
  585.         alfa=alfa-hilfe;
  586.         return alfa;
  587.     }
  588.     else
  589.     {
  590.         alfa=alfa+hilfe;
  591.         return alfa;
  592.     }
  593. }
  594.  
  595. //ermittelt aus x und y Wert den beta winkel
  596. float GetRobotBeta(float xWelt,float yWelt)
  597. {
  598.     double a,b,c;
  599.     float beta;
  600.  
  601.     //seitenlänge
  602.     a=sqrt(((xWelt-ober_arm.x1)*(xWelt-ober_arm.x1))+((yWelt-ober_arm.y1)*(yWelt-ober_arm.y1)));
  603.     b=unter_arm.radius;
  604.     c=ober_arm.radius;
  605.  
  606.     //BetaWinkel
  607.     beta=((c*c)+(b*b)-(a*a));
  608.  
  609.     if(beta!=0)
  610.     {
  611.         beta=beta/(2*b*c);
  612.         beta=(acos(beta)*180)/M_PI;
  613.         return beta;
  614.     }
  615.  
  616.     return beta;
  617. }
  618.  
  619. //Bewegt den Roboterarm an eine gewünschte Position
  620. bool BewegeRobot(float xStart, float yStart, float xEnde, float yEnde,int modi, viewPortStruct viewPort)
  621. {
  622.     double a;
  623.  
  624.     //seitenlänge
  625.     a=sqrt(((xStart-ober_arm.x1)*(xStart-ober_arm.x1))+((yStart-ober_arm.y1)*(yStart-ober_arm.y1)));
  626.    
  627.     if(a!=0 && !(a>ober_arm.radius+unter_arm.radius) && !(a<ober_arm.radius-unter_arm.radius))
  628.     {
  629.         //seitenlänge
  630.         a=sqrt(((xEnde-ober_arm.x1)*(xEnde-ober_arm.x1))+((yEnde-ober_arm.y1)*(yEnde-ober_arm.y1)));
  631.  
  632.         if(a!=0 && !(a>ober_arm.radius+unter_arm.radius) && !(a<ober_arm.radius-unter_arm.radius))
  633.         {
  634.             if(modi==0)
  635.             {
  636.                 BewegeLienar(GetRobotAlfa(xStart,yStart),
  637.                         GetRobotBeta(xStart,yStart),
  638.                         GetRobotAlfa(xEnde,yEnde),
  639.                         GetRobotBeta(xEnde,yEnde),
  640.                         viewPort);
  641.  
  642.                         return true;
  643.             }
  644.             else
  645.             {
  646.                 BewegeSequent(GetRobotAlfa(xStart,yStart),
  647.                         GetRobotBeta(xStart,yStart),
  648.                         GetRobotAlfa(xEnde,yEnde),
  649.                         GetRobotBeta(xEnde,yEnde),
  650.                         viewPort);
  651.                 return true;
  652.             }
  653.         }
  654.         return false;
  655.     }
  656.  
  657.     return false;
  658. }
  659.  
  660. //steurungsinfo
  661. void infotxt(void)
  662. {
  663.     char Text1[]="Tasten Belegung";
  664.     char Text2[]="'B'-Beenden 'D'-Direkte Steuerung, 'E'-Eingabe, 'R'-Reset";
  665.     char Text3[]="'4'-Links '6'-Rechts '2'-Runter '8'-Hoch";
  666.     char Text4[300];
  667.    
  668.     sprintf(Text4,"Eingabegrenzen: X [%.0f, %.0f], Y [%.0f, %.0f]",
  669.         ((ober_arm.radius+unter_arm.radius)*(-1)),
  670.         (ober_arm.radius+unter_arm.radius),
  671.         (ober_arm.radius+unter_arm.radius),
  672.         (unter_arm.radius)*(-1));
  673.  
  674.  
  675.     VpText(XWeltToViewPort(10,welt3,vPort3),
  676.         YWeltToViewPort(115,welt3,vPort3),
  677.         Text1,vPort3);
  678.  
  679.     VpText(XWeltToViewPort(10,welt3,vPort3),
  680.         YWeltToViewPort(105,welt3,vPort3),
  681.         Text2,vPort3);
  682.  
  683.     VpText(XWeltToViewPort(10,welt3,vPort3),
  684.         YWeltToViewPort(95,welt3,vPort3),
  685.         Text3,vPort3);
  686.  
  687.     VpText(XWeltToViewPort(10,welt3,vPort3),
  688.         YWeltToViewPort(85,welt3,vPort3),
  689.         Text4,vPort3);
  690. }
  691.  
  692. //Lies die eingaben ein und speichert sie in die Struktur
  693. void read_key(char *txt, char *strukt,int zeile, viewPortStruct viewPort)
  694. {
  695.     int komma=0;
  696.     int vorzeichen=0;
  697.     int minus=0;
  698.     int eingabe;
  699.     char zahl[30];
  700.     strcpy(strukt, "");
  701.     strcpy(zahl, "");
  702.  
  703.     do
  704.     {
  705.         eingabe=getch();
  706.         switch(eingabe)
  707.         {
  708.             case 45:
  709.                 if(vorzeichen==0)
  710.                 {
  711.                     strcpy(zahl, "");
  712.                     sprintf(zahl,"%c", eingabe);
  713.                     strcat(strukt,zahl);
  714.  
  715.                     VpText(XWeltToViewPort(10+textwidth(txt),welt3,viewPort),
  716.                         YWeltToViewPort(zeile,welt3,viewPort),
  717.                         strukt,viewPort);
  718.  
  719.                     minus=1;
  720.                     ++vorzeichen;
  721.                 };break;
  722.             case 46:
  723.                 if(komma==0)
  724.                 {
  725.                     if((minus==0 && vorzeichen>0) || (minus==1 && vorzeichen>1))
  726.                     {
  727.                         strcpy(zahl, "");
  728.                         sprintf(zahl,"%c", eingabe);
  729.                         strcat(strukt,zahl);
  730.    
  731.                         VpText(XWeltToViewPort(10+textwidth(txt),welt3,viewPort),
  732.                             YWeltToViewPort(zeile,welt3,viewPort),
  733.                             strukt,viewPort);
  734.  
  735.                         komma=1;
  736.                         ++vorzeichen;
  737.                     }
  738.                 };break;
  739.             case 48: case 49: case 50:
  740.             case 51: case 52: case 53:
  741.             case 54: case 55: case 56:
  742.             case 57:
  743.                 strcpy(zahl, "");
  744.                 sprintf(zahl,"%c", eingabe);
  745.                 strcat(strukt,zahl);
  746.  
  747.                     VpText(XWeltToViewPort(10+textwidth(txt),welt3,viewPort),
  748.                         YWeltToViewPort(zeile,welt3,viewPort),
  749.                         strukt,viewPort);
  750.  
  751.                 ++vorzeichen;
  752.                 break;
  753.         }      
  754.     } while(eingabe!=13);
  755. }
  756.  
  757. //Liest die werte über andere funktionen ein und ruft bewege robot auf
  758. bool Lese_Keyboard(void)
  759. {
  760.     char Text1[300]="Bitte geben Sie die Start x und y Werte ein: X:";
  761.     char Text2[300]="Bitte geben Sie die End   x und y Werte ein: X:";
  762.     char Text3[300]="Fuer Lineare Bewegung '0' sonst '1':      Modi:";
  763.  
  764.    
  765.     VpReset(vPort3);
  766.     VpShow(vPort3);
  767.  
  768.     infotxt();
  769.  
  770.     int zeile=30;
  771.  
  772.     //xStart yStart
  773.     VpText(XWeltToViewPort(10,welt3,vPort3),
  774.         YWeltToViewPort(zeile,welt3,vPort3),
  775.         Text1,vPort3);
  776.  
  777.     read_key(Text1, Input.ex1,zeile, vPort3);
  778.     strcat(Text1,Input.ex1);
  779.     strcat(Text1," Y:");
  780.  
  781.     VpText(XWeltToViewPort(10,welt3,vPort3),
  782.         YWeltToViewPort(zeile,welt3,vPort3),
  783.         Text1,vPort3);
  784.  
  785.     read_key(Text1, Input.ey1, zeile, vPort3);
  786.     //xyStart-ende
  787.  
  788.     zeile=zeile-10;
  789.  
  790.     //xEnde yEnde
  791.     VpText(XWeltToViewPort(10,welt3,vPort3),
  792.         YWeltToViewPort(zeile,welt3,vPort3),
  793.         Text2,vPort3);
  794.  
  795.     read_key(Text2, Input.ex2,zeile, vPort3);
  796.     strcat(Text2,Input.ex2);
  797.     strcat(Text2," Y:");
  798.  
  799.     VpText(XWeltToViewPort(10,welt3,vPort3),
  800.         YWeltToViewPort(zeile,welt3,vPort3),
  801.         Text2,vPort3);
  802.  
  803.     read_key(Text2, Input.ey2, zeile, vPort3);
  804.     //xyStart-ende
  805.  
  806.     zeile=zeile-10;
  807.     //modi Start
  808.     VpText(XWeltToViewPort(10,welt3,vPort3),
  809.         YWeltToViewPort(zeile,welt3,vPort3),
  810.         Text3,vPort3);
  811.  
  812.     read_key(Text3, Input.modi,zeile, vPort3); 
  813.     //modi ende
  814.     if(textwidth(Input.ex1)>0 && textwidth(Input.ey1)>0 && textwidth(Input.ex2)>0 && textwidth(Input.ey2)>0 && textwidth(Input.modi)>0)
  815.     {
  816.         if(!BewegeRobot(atof(Input.ex1),atof(Input.ey1),atof(Input.ex2),atof(Input.ey2),atof(Input.modi),vPort1))
  817.         {
  818.             //meldung
  819.             VpText(XWeltToViewPort(ober_arm.x1-50,welt1,vPort1),
  820.                 YWeltToViewPort(ober_arm.y1-50,welt1,vPort1),
  821.                 ober_arm.text,
  822.                 vPort1);
  823.             return true;
  824.         }
  825.         return false;
  826.     }
  827.     else
  828.     {
  829.         //meldung
  830.         VpText(XWeltToViewPort(ober_arm.x1-50,welt1,vPort1),
  831.             YWeltToViewPort(ober_arm.y1-50,welt1,vPort1),
  832.             ober_arm.text,
  833.             vPort1);
  834.     }
  835.     return false;
  836. }
  837.  
  838. //ERSTE AUFRUF IM PROGRAMM
  839. void Main_Init(void)
  840. {
  841.     //ViewPorts INITIALISIEREN UND ZEICHNEN
  842.     InitViewPort1();
  843.     InitViewPort2();
  844.     InitViewPort3();
  845.  
  846.     //Roboterarm initialisieren
  847.     ober_arm.radius = 100;
  848.     unter_arm.radius = ((ober_arm.radius*70)/100);
  849.  
  850.     strcpy(ober_arm.text,"Nicht Ausfuehrbar");
  851.  
  852.     //WELTKOODINATE MIN MIX FÜR 3VP's
  853.     //Weltkoordinaten punkte für ViewPort1
  854.     welt1.xmax=ober_arm.x1+(ober_arm.radius+unter_arm.radius)+10;
  855.     welt1.xmin=ober_arm.x1-(ober_arm.radius+unter_arm.radius)-10;
  856.     welt1.ymax=ober_arm.y1+(ober_arm.radius+unter_arm.radius)+10;
  857.     welt1.ymin=ober_arm.y1-(ober_arm.radius+unter_arm.radius)-10;
  858.    
  859.     //Weltkoordinaten punkte für ViewPort2
  860.     welt2.xmin = -40;
  861.     welt2.xmax = 230;
  862.     welt2.ymin = -40;
  863.     welt2.ymax = 230;
  864.    
  865.     //Weltkoordinaten punkte für ViewPort3
  866.     welt3.xmin=0;
  867.     welt3.xmax=vPort3.right-vPort3.left;
  868.     welt3.ymin=0;
  869.     welt3.ymax=vPort3.bottom-vPort3.top;
  870.  
  871.     //Roboterarm Usrpsrung
  872.     ober_arm.x1 = 0;
  873.     ober_arm.y1 = 0;
  874.  
  875.     //Winkelfestlegen
  876.     alpha_beta.alfa = 90;
  877.     alpha_beta.beta = 90;
  878.  
  879.     //Werte Für Koordinatenachse Init..
  880.     alpha_beta.faktor = -10;
  881.     alpha_beta.x1 = 0;
  882.     alpha_beta.x2 = alpha_beta.x1+180;
  883.     alpha_beta.y1 = alpha_beta.x1;
  884.     alpha_beta.y2 = alpha_beta.y1+180;
  885.     alpha_beta.farbe = 0;
  886.  
  887.     //Koordianten Achsen Zeichnen
  888.     kord_achsen();
  889.  
  890.     //Steuerungs info
  891.     infotxt();
  892.  
  893.     //Roboterarm für Start Zeichnen
  894.     ZeichneRobot(alpha_beta.alfa, alpha_beta.beta, vPort1);
  895.  
  896. }
  897.  
  898. //Steuerung mit Tastatur
  899. void Steuerung(void)
  900. {
  901.     int beenden=0, d_beenden=0;
  902.  
  903.     while (beenden==0)
  904.     {
  905.         int taste;
  906.         taste=getch();
  907.  
  908.         //b oder B taste drücken damit das Programm Beendet wird
  909.         if (taste==66 || taste==98 || taste==27)
  910.         {
  911.                     beenden=1;
  912.         }
  913.  
  914.         //R TASTE
  915.         if (taste==82 || taste==114)
  916.         {
  917.             VpReset(vPort1);
  918.             VpReset(vPort2);
  919.             VpReset(vPort3);
  920.             Main_Init();
  921.         }
  922.  
  923.         //E TASTE
  924.         if (taste==69 || taste==101)
  925.         {
  926.             Lese_Keyboard();
  927.         }
  928.  
  929.         //D oder D drücken damit man Direktsteuerungs modus kommt
  930.         if (taste==68 || taste==100)
  931.         {
  932.             d_beenden=0;
  933.             while (d_beenden==0)
  934.             {
  935.                 taste=getch();
  936.                 //D Drücken um zu verlassen (DIREKT)
  937.                 if (taste==68 || taste==100 || taste==27)
  938.  
  939.                 {
  940.                     d_beenden=1;
  941.                 }
  942.  
  943.                 if (taste==50)
  944.                 {
  945.                     //Unterrarm Bewegen Grad-
  946.                     int winkel=alpha_beta.beta;
  947.                     --winkel;
  948.  
  949.                     if (!(ZeichneRobot(alpha_beta.alfa,winkel,vPort1)))
  950.                     {
  951.                         ++winkel;
  952.                         alpha_beta.beta=winkel;
  953.                     }
  954.                 }
  955.  
  956.                 if (taste==56)
  957.                 {
  958.                     //Unterarm Bewegen Grad+
  959.                     int winkel=alpha_beta.beta;
  960.                     ++winkel;
  961.  
  962.                     if (!(ZeichneRobot(alpha_beta.alfa,winkel,vPort1)))
  963.                     {
  964.                         --winkel;
  965.                         alpha_beta.beta=winkel;
  966.                     }
  967.                 }
  968.  
  969.                 if (taste==54)
  970.  
  971.                 {
  972.                     //Oberrarm Bewegen Grad-
  973.                     int winkel=alpha_beta.alfa;
  974.                     --winkel;
  975.  
  976.                     if (!(ZeichneRobot(winkel,alpha_beta.beta,vPort1)))
  977.                     {
  978.                         ++winkel;
  979.                         alpha_beta.alfa=winkel;
  980.                     }
  981.  
  982.                 }
  983.  
  984.                 if (taste==52)
  985.                 {
  986.                     //Oberarm Bewegen Grad+
  987.                     int winkel=alpha_beta.alfa;
  988.                     ++winkel;
  989.  
  990.                     if (!(ZeichneRobot(winkel,alpha_beta.beta,vPort1)))
  991.                     {
  992.                         --winkel;
  993.                         alpha_beta.alfa=winkel;
  994.                     }
  995.                 }
  996.             }
  997.         }
  998.     }
  999.  
  1000.     //Grafikmodus Verlassen
  1001.     closegraph();
  1002. }
Add Comment
Please, Sign In to add comment
Public Pastes
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!