hi danke für den antwort ja bin zum ersten mal hier wie gesagt hab bis
jetzt noch nie mit controllern zutun gehabt aber will ich ja jetzt mal
versuchen... das ist das teil des codes wo ich was optimieren muss damit
die greifer auf und zu gehen ich kopiere dann den ganzen code.
#include <91x_lib.h>
#include <stdlib.h>
#include <stdio.h> // Standard I/O Functions
//#include "measure.h"
#include <math.h>
/* Konstanten für Sprachmodul*/
#define kreis 0x04 // P5.2
#define dreieck 0x01 // P5.0
void SetOutputWert (void)
#define schwarze_tasse 0x20 // P5.5
#define weisse_tasse 0x10 // P5.4
/* Konstanten für handshaking UART*/
#define x 0x78
#define y 0x79
#define s 0x73
#define w 0x77
#define k 0x6b
#define d 0x64
#define a 0x61
#define b 0x62
#define S2 0x20 /* P3.5 (INT5) */
#define S3 0x40 /* P3.6 (INT6) */
#define Kontrolloutput 0xFF
#define Programmende 0x01
#define q 0x71 /*Konstante die gesendet wird wenn kein Objekt
identifiziert wurde*/
unsigned char InputStatus;
unsigned char OutputWert;
unsigned char Koordx = 0x00; /*Koordx und Koordy sind die von
Bildverarbeitung gelieferten Koornidaten*/
unsigned char Koordy = 0x00;
unsigned char Decision; /*Aufgrund dieser Variable wird die
Bewegungsentscheidung getroffen*/
unsigned char Variable = 0x00;
const char xnull = 0x7F; /*255/2 ungefähr = 127*/
const char ynull = 0x5F; /*191/2 ungefähr = 95*/
/*Ansteuerung der Laufzeit des Roboterarms*/
void zeit1 (unsigned char Koordx, unsigned char Koordy)
{
double n,m;
n=(atan2((Koordx-139),(Koordy+28)))*800000;
for (m=0;m<n;m++);
}
void zeit2 (unsigned char Koordx, unsigned char Koordy)
{
double n,m;
n=(atan2((139-Koordx),(Koordy+28)))*800000;
for (m=0;m<n;m++);
}
void zeit3 (unsigned char Koordx, unsigned char Koordy) //
Laufzeit der Unterarm nach unten
{
double n,i;
int m,p;
i=(476-(((139-Koordx)*(139-Koordx)*0.01)+(((Koordy+28)*(Koordy+28))*0.01
)))/600;
n=(180-(acos(i)*57.5));
n=n*740000;
p=(int)n;
for (m=0;m<p;m++);
}
void zeit4 (unsigned char Koordx, unsigned char Koordy) //
Laufzeit der Oberarm nach unten
{
double n,i,u,e,r,l;
int m,p;
l=sqrt(256+(Koordx-139)*(Koordx-139)*0.01+(Koordy+28)*(Koordy+
28)*0.01);
r=((Koordx-139)*(Koordx-139)*0.01+(Koordy+28)*(Koordy+28)*0.01-163.75)/(
25*l);
i=acos(r)*57.5;
e=sqrt((Koordx-139)*(Koordx-139)*0.01+(Koordy+28)*(Koordy+28)*0.01);
u=(atan2(e,16))*57.5;
n=(180-i-u)*650000;
p=(int)n;
for (m=0;m<p;m++);
}
void zeit5 (unsigned char Koordx, unsigned char Koordy) //
Laufzeit der Oberarm nach oben
{
double n,i,u,e,r,l;
int m,p;
l=sqrt(256+(Koordx-139)*(Koordx-139)*0.01+(Koordy+28)*(Koordy+28)*0.01);
r=((Koordx-139)*(Koordx-139)*0.01+(Koordy+28)*(Koordy+28)*0.01-163.75)/(
25*l);
i=acos(r)*57;
e=sqrt((Koordx-139)*(Koordx-139)*0.01+(Koordy+28)*(Koordy+28)*0.01);
u=(atan2(e,16))*57;
n=(180-i-u)*750000;
p=(int)n;
for (m=0;m<p;m++);
}
void zeit6 (unsigned char Koordx, unsigned char Koordy) // Laufzeit
der Unterarm nach oben
{
double n,i;
int m,p;
i=(476-(((139-Koordx)*(139-Koordx)*0.01)+(((Koordy+28)*(Koordy+28))*0.01
)))/600;
n=(180-(acos(i)*57));
n=n*800000;
p=(int)n;
for (m=0;m<p;m++);
}
/*Buttonmodul*/
void GetInputStatus (void) /*Buttonmodul*/
{
InputStatus = 0x00;
if (GPIO3->DR[S2*4] == 0) /* S2 pressed means 0 */
InputStatus |= k;
if (GPIO3->DR[S3*4] == 0) /* S3 pressed means 0 */
InputStatus |= d;
}
//void GetInputStatus (void)
//{
//InputStatus = 0x00; //Anfangswert 0,es wurde noch kein Wort
gesprochen
//if (GPIO5->DR[kreis*4] == 0) /* Wort 1 gesprochen */
// InputStatus |= k;
//if (GPIO5->DR[dreieck*4] == 0) /* Wort 2 gesprochen */
// InputStatus |= d;
//if (GPIO5->DR[weisse_tasse*4] == 0) /* Wort 3 gesprochen */
// InputStatus |= w;
//if (GPIO5->DR[schwarze_tasse*4] == 0) /* Wort 4 gesprochen */
// InputStatus |= s;
//}
void SetOutputWert (void)
{
GPIO7 -> DR[0xFF*4] = OutputWert; /*Visualisierung der ASCII Code für
den gewünschten Objekt*/
}
int decide (void) // Entscheidung, welcher Quadrant angefahren wird
{
if (Koordx < xnull) Decision = 1 ; /*2ten Quadrant*/
if (Koordx > xnull) Decision = 2 ; /*1ten Quadrant*/
if (Koordx == 0x71 && Koordy == 0x71) Decision = 3 ; /*kein Objekt*/
return Decision;
}
void execute(void) /*Programm des Boards*/
{
int m;
switch(Decision)
{
case 1: /*naheplan-rechts*/ // Wenn Kreiss, Dreieck oder Viereck
in Links liegen
GPIO7->DR[0xFF*4] = 0x10; //P7.4 Drehung rechts
zeit2(Koordx,Koordy);
GPIO7->DR[0xFF*4] = 0x0C; //P7.2 & P7.3 Unterarm nach unten
zeit3(Koordx,Koordy);
GPIO7->DR[0xFF*4] = 0x01; //P7.0 Oberarm nach unten
zeit4(Koordx,Koordy);
GPIO7->DR[0xFF*4] = 0x00; //Stop
for (m=0; m<10000000; m++);
GPIO7->DR[0xFF*4] = 0x30; //P7.4 & P7.5 Drehung links
zeit2(Koordx,Koordy);
GPIO7->DR[0xFF*4] = 0x03 ; // Oberarm nach oben
zeit5(Koordx,Koordy);
GPIO7->DR[0xFF*4] = 0x04 ; // Unterarm nach oben
zeit6(Koordx,Koordy);
GPIO7->DR[0xFF*4] = 0x00; //Stop
break;
case 2: /*naheplan-links*/ //Wenn Kreiss, Dreieck oder Viereck in
Rechts liegen
GPIO7->DR[0xFF*4] = 0x40;
GPIO7->DR[0xFF*4] = 0xc0;
GPIO7->DR[0xFF*4] = 0x30; //P7.4 & P7.5 Drehung links
zeit1(Koordx,Koordy);
GPIO7->DR[0xFF*4] = 0x0C; //P7.2 & P7.3 Unterarm nach unten
zeit3(Koordx,Koordy);
GPIO7->DR[0xFF*4] = 0x01; //P7.0 Oberarm nach unten
zeit4(Koordx,Koordy);
GPIO7->DR[0xFF*4] = 0x00; //Stop
for (m=0; m<10000000; m++);
GPIO7->DR[0xFF*4] = 0x10; //P7.4 Drehung rechts
zeit1(Koordx,Koordy);
GPIO7->DR[0xFF*4] = 0x03; // Oberarm nach oben
zeit5(Koordx,Koordy);
GPIO7->DR[0xFF*4] = 0x04 ; // Unterarm nach oben
zeit6(Koordx,Koordy);
GPIO7->DR[0xFF*4] = 0x00; //Stop
break;
case 3: /*Kein Objekt*/
//GPIO7->DR[0xFF*4] = 0x00; //Stop
//for (m=0; m<10000000; m++);
//break;
}
}
/*Kamerasystem*/
int main (void) /*Hauptprogramm*/
{
{
//init_serial ();
/*Visualisierungsport*/
SCU->GPIOOUT[7] = 0x5555; //P7 Output Mode 1
GPIO7->DDR = 0xFF; //Alle Portpins auf Output
GPIO7->DR[0xFF*4] = 0x00; //Alle Portpins auf Input
GPIO7->DR[0xFF*4] = 0x00;
SCU->GPIOOUT[5] = 0x0000; //P5 Output Mode 0
GPIO5->DDR = 0x00; //Alle Portpins auf Input
Koordx = 0xAF;
Koordy = 0x8F;
// SCU->GPIOOUT[3] = 0x0000; /* P3.0..7 no output
*/
//GPIO3->DDR = 0x9F; /* P3.0..7 Input (Push
Buttons) */
/*Sprachmodulinput*/
decide ();
execute();
InputStatus=0x00;
Koordx = 0x00; //Anfangswerte=0
Koordy = 0x00;
}
}
das mit dem bild verarbeitung und sprach module habe ich für den anfang
gelassen
hoffe vesteht man ja jetzt was in dem Code