IR-radarsysteem (3 / 3 stap)

Stap 3: Broncode

#define GLCD_WIDTH 128

#define GLCD_E PIN_E0 / / Enable
#define GLCD_DI PIN_B5

#define GLCD_CS1 PIN_A1 / / Chip selectie 1
#define GLCD_CS2 PIN_B0

#define SERVO PIN_A3

#include < HDM64GS12.c >
#include < graphics.c >
#include < math.h >

char [message] = "EndtaS RaDaR";
zweven theta = 0, ytheta;

void main() {}

INT1 enbas;
int mesafe;
int car1, sayac, een;
INT16 servopos;

setup_adc_ports(RA0_ANALOG);
setup_adc(ADC_CLOCK_INTERNAL);
set_adc_channel(0);
enbas:
glcd_init(on);

glcd_fillScreen(off);

glcd_text57 (30, 0, bericht, 1, 1);

glcd_circle (29, 37, 26, 0, 1);
glcd_pixel (29, 37, ON);
Theta = 7.85;
servopos = 550;
printf("Scan_START\r\n");
while (true) {}

mesafe = read_adc();
printf ("th: %f -> %u \r\n\n",theta, mesafe);

ytheta = theta;
sayac = 1;
Als (enbas == TRUE) {}
sayac = 50;
}
voor (een = 0; een < sayac; a ++) {}
output_high(Servo);
delay_us(servopos);
output_low(Servo);
delay_us (20000 - servopos);
}
Als (enbas == TRUE) {}
glcd_circle (29, 37, 25, 1, 0);
glcd_pixel (29, 37, ON);
printf("Scan_START\r\n");
delay_ms(300);
}

Als (ytheta > = 4,51) {}
ytheta-=. 02;
servopos += 12;
enbas = FALSE;
}
else {}
glcd_circle (29, 37, 25, 1, 0);
ytheta = 7.85;
servopos = 550;
glcd_pixel (29, 37, ON);
enbas = TRUE;
printf("Scan_COMPLETE\r\n");
}

glcd_pixel (28+(int)(car1*sin(-theta)+.5), 38-(int)(car1*cos(theta)+.5), OFF);

glcd_pixel (28+(int)(car1*sin(-theta-1)+.5), 38-(int)(car1*cos(theta-1)+.5), OFF);

Theta = ytheta;

car1 = mesafe / 8;
glcd_pixel (28+(int)(car1*sin(-theta)+.5), 38-(int)(car1*cos(theta)+.5), ON);
delay_ms(10);
}
}