int main()
{
int i;
init(0);
// connect camera to the screen
open_screen_stream();
// set all didgital outputs to +5V
for (i = 0; i < 8; i++)
{
// set all digital channels as outputs
select_IO(i,0);
write_digital(i,1);
}
while(1)
{
motorControl(line());
}
// terminate hardware
close_screen_stream();
set_motor(1,0);
set_motor(2,0);
return 0;
}
int main() {
init(0);
open_screen_stream();
int edgeDetectedImage[320][240];
int prevEdgeDetected[320][240];
double correlation[32][32];
while(1)
{
take_picture();
convert_camera_to_screen();
for(int i=16;i<320-16;i++) {
for(int j=16;j<240-16;j++) {
for(int k=-16;k<16;k++) {
for(int l=-16;l<16;l++) {
correlation[k+16][l+16] = sqrt((double)(get_pixel(i,j,3)*prevEdgeDetected[i+k][j+l]));
}
}
}
}
for(int k=0;k<32;k++) {
for(int l=0;l<32;l++) {
put_pixel(k+400,l+400,correlation[k][l],0,0);
}
}
update_screen();
for(int i=0;i<320;i++) {
for(int j=0;j<240;j++) {
prevEdgeDetected[i][j] = edgeDetectedImage[i][j];
}
}
}
}
int display_picture(int delay_sec,int delay_usec)
{
open_screen_stream();
// copy original sceen fb to "original" buffer
memcpy(&(original[0]),fbp,screensize);
// original screen -> cam_disp
// memcpy(&(cam_disp[0]),fbp,screensize);
// update camera portion of the screen
update_screen();
sleep(delay_sec);
usleep(delay_usec);
// put original image back
memcpy(fbp,&(original[0]),screensize);
close_screen_stream();
// cleanup
return 0;
}
int main(){
init(0);
int numfound;
int numfoundleft;
int numfoundright;
int pixel;
int pixell;
int pixelr;
int THRESH = 110;
while(true){
numfound = 0;
numfoundleft = 0;
numfoundright = 0;
take_picture();
open_screen_stream();
for(int i = 0; i < 320; i++){
pixel = get_pixel(i, 40, 3);
set_pixel(i, 40, 0, 0, 254);
if(pixel > THRESH){
numfound++;
}
}
for(int o = 0; o<240; o++){
pixell = get_pixel(40, o, 3);
pixelr = get_pixel(280, o, 3);
set_pixel(40, o, 0, 254, 0);
set_pixel(280, o, 254,0,0);
if(pixell > THRESH){
numfoundleft++;
}
if(pixelr > THRESH){
numfoundright++;
}
}
update_screen();
printf("num found: %d \n", numfound);
printf("num found left: %d \n", numfoundleft);
printf("num found right: %d \n", numfoundright);
}
return 0;
}
int main() {
init(0);
//Networking Code
connect_to_server("130.195.6.196",1024);
send_to_server("Please");
char message[24];
receive_from_server(message);
send_to_server(message);
//Signal Catcher code
signal(2, signal_callback_handler);
//Declaration of Constants for 2nd Quadrant
double errorSum = 0;
double errorSum2 = 0;
double kp = 0.5;
double kd = 5;
double proportional_signal;
open_screen_stream();
int quad_three = 0;
printf("QUADRANT ONE AND TWO\n");
while(1) {
quad_three = 0;
take_picture();
//CODE FOR PROPORTIONAL
for (int i=0; i < 320; i++){
if(get_pixel(i, 160, 3) > THRESH) {
errorSum += (i-160);
quad_three++;
}
}
//CODE FOR FUTURE LINE ERROR
for (int i=0; i < 320; i++){
if(get_pixel(i, 80, 3) > THRESH) {
errorSum2 += (i-160);
}
}
errorSum/=160;
errorSum2/=160;
//THIS IS NOT ACTUALLY DERIVATIVE SIGNAL - THIS CODE:
//Looks to see if there any curves in the line and slows down based on how curvy the curve is
int derivative_signal = abs(errorSum-errorSum2)*kd;
if(derivative_signal > 33) {
derivative_signal = 33;
}
proportional_signal = errorSum * kp;
int leftSpeed = BASE_SPEED + proportional_signal - derivative_signal;
int rightSpeed = BASE_SPEED - proportional_signal - derivative_signal;
if (proportional_signal < - 0.5 && proportional_signal > -0.6 && quad_three != 320){
for(int i = 0; i < 10; i++) {
move(BASE_BACK_SPEED, BASE_BACK_SPEED);
}
} else {
if(quad_three == 320)break;
move(leftSpeed, rightSpeed);
}
}
printf("THIRD QUADRANT\n");
move(50,50);
Sleep(0,200000);
while(1) {
if (p(160,0.3,50,0) > 310 && p(40,0.3,50,0) < 80) {
turn(1);
break;
}
}
int red = 0;
int blue = 0;
int green = 0;
while(red < 200 || green > 100 || blue > 100) {
take_picture();
red = get_pixel(120,160,0);
green = get_pixel(120,160,1);
blue = get_pixel(120,160,2);
int p160 = p(160,0.7,40,0);
int p80 = p(80,0.7,40,0);
if (p160 > 310 && p80 < 80) {
turn(1);
}
}
move(70,70);
Sleep(0,200000);
int same_count = 0;
double prev_signal = 0;
int count = 0;
int prevTotal = 0;
while (1){
int left_signal = read_analog(1);
int right_signal = read_analog(0);
int base_speed = 45;
double constant = 0.075;
/*printf("Left %d\n",left_signal);
printf("Right %d\n",right_signal);*/
double actual_signal = left_signal - right_signal;
actual_signal *= constant;
actual_signal += 4.8;
printf("Actual %f\n",actual_signal);
if (actual_signal > 20 || actual_signal < -20){
base_speed = 35;
} else {
base_speed = 45;
}
move(base_speed + actual_signal, (base_speed - actual_signal)*1.3);
}
}
int main()
{
//Open gate:
//connects to server
//connect_to_server("130.195.6.196", 1024);
//sends a message to the connected server
//send_to_server("Please");
//send_to_server("123456");
//receives message from the connected server
//char message[24];
//receive_from_server(message); //this line looks buggy, is it right?
//send_to_server(message);
int i = 0; // Re-added this because didnt seem to complie without it
int baseSpeed = 35;
float kp = 0.03; //random constant
float kd = 0;
init(0);
// connect camera to the screen
open_screen_stream();
// set all didgital outputs to +5V
for (int i = 0; i < 8; i++)
{
// set all digital channels as outputs
select_IO(i,0);
write_digital(i,1);
}
while(1)
{
take_picture(); // take camera shot
int white[320];
int value;
// we made the array 320 because that is the width of pixels
// draw some line
for(int i = 0; i < 320; i++){
set_pixel(i, 55 ,255,0,0);//redline
value = get_pixel(i,56,3); // give each pixel in the array the pixel value of its location based on ' i '.
if(value > 100){ // change 70 to actual white line value later
white[i] = 1;
}
else{
white[i] = 0;
}
//printf("%d\n",white[i]); // print array results
}
//process the data collected so far:
//but first create an array counting from -170 to 170
int e=0;
int sum = 0;
for(int i=0;i<320;i++){
sum = sum + (i - 160)*white[i];
}
int previous_error; //-We removed the initialisation of this to 0 because it would be set to 0 every time it looped.
// think as it is now, the previous error will always be zero when the wile loop goes round.
int current_error = 0;
int derivative_signal;
int error = 0;
for(int i=0;i<320;i++){ // This looks very similar to what we already have above to to the proportional error
error = (i - 160)*white[i]; // Maybe we can combine these in some way?
current_error += error;
}
//Sleep(0,100000); // This could be a problem - removed because we already have a sleep command
derivative_signal = (current_error-previous_error/0.1)*kd; //I think this si the equivalent of 'e = (sum/20)*kp;' below
previous_error = current_error;
printf("Derivative signal is: %d", derivative_signal );
e = sum*kp;
int LM = baseSpeed+e+derivative_signal;
int RM = baseSpeed+(-1*e)+(-1*derivative_signal);
printf("%d\n",LM);
set_motor(1, LM);
set_motor(2, RM);
// display picture
update_screen();
Sleep(0,100000);
for (i = 0 ; i < 8; i++)
{
int av = read_analog(i);
// printf("ai=%d av=%d\n",i,av);
}
}
// terminate hardware
close_screen_stream();
set_motor(1,0);
set_motor(2,0);
return 0;
}
