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(1);
int x;
connect_to_server("130.195.6.196", 1024); //Opening the gate code
send_to_server("Please");
char message[24];
receive_from_server(message);
printf("%s", message);
send_to_server(message);
for (x = 0; x < 8; x++)
{
select_IO(x,0);
write_digital(x,1);
}
while(1){
take_picture();
int sum = 0;
float kp = 0.5;
int w;
int s;
int proportional_signal=0;
for(int x = 0; x < 320; x++){
w = get_pixel(x,120,3);
if(w>127){s=1;}
else{s=0;};
sum = sum + (x-160)*s;}
proportional_signal = sum * kp;
if (sum < 0){
set_motor(1, proportional_signal);
set_motor(2, -1*proportional_signal);
}
else if (sum > 0){
set_motor(1, -1*proportional_signal);
set_motor(2, proportional_signal);
}
else{
set_motor(1, proportional_signal);
set_motor(2, proportional_signal);
}
update_screen();
for (x = 0 ; x < 8; x++)
{
int adc_reading = read_analog(x);
printf("ai=%d av=%d\n",x,adc_reading);
}
}
set_motor(1,0);
set_motor(2,0);
return 0;
}
int main(){
//This sets up the RPi hardware and ensures everything is working properly
init(0);
//connect_to_server("130.195.6.196", 1024);
//sends message
//send_to_server("Please");
//receives message
//char message[24];
//receive_from_server(message);
//send_to_server(message);
select_IO(2, 1); //right sensor
select_IO(4, 1); //left sensor
//right wheel
set_motor(1, minSpeed);
// left wheel
set_motor(2, minSpeed);
//infinite loop for testing purposes
while(true){
//Take picture with camera
take_picture();
prevError = error;
error=0;
errorR=0;
errorL=0;
sum=0;
red=0;
for (int i=0; i<160; i++){
w = get_pixel(i, 120, 3);
r=get_pixel(i,120,0);
if(r>210){
red++;
}
if(w > 120){
errorL++;
}
}
for (int i=160; i<320; i++){
//this is calculating the sum for the whole width of pixels as opposed to just right, there is no range?
// possibly the condition for the loop should be
//for (int i=0; i<320 && i>160; i++){
w = get_pixel(i, 120, 3);
r=get_pixel(i,120,0);
if(r>210){
red++;
}
if(w > 120){
errorR++;
}
}
error=errorR-errorL;
//rests for 0.1 seconds
Sleep(0,1000);
sum=errorR+errorL;
//when it reaches the secount quadrant
if((sum)>310){
//this could be made true when doing the first set of curves, causing it to break into the
First=0;
//intersection code
minSpeed=70;
}
int test=read_analog(2);
int test1=read_analog(4);
if(First==0&&test>400&&test1>400){
First=2;
minSpeed = 60;
}
if(First==2){
prevError=0;
break;
}
if((sum>10)&&First==1){
//Proportional Signal
propSignal = (error)*Kp;
propSignal=(propSignal/160)*200;
//Integral Signal
sumError += error;
intSignal = (sumError)*Ki;
//trial and error find an x value that works
derSignal = ((error-prevError)/sleepTime)*Kd;
//right wheel
set_motor(1, minSpeed + (propSignal + intSignal + derSignal));
// left wheel
set_motor(2, minSpeed - (propSignal + intSignal + derSignal));
}else if(First==1){ //Do we need this, this could be causing the initial spin
//turns until line is found.
set_motor(1, -50);
set_motor(2, -60);
//when it reaches the intersections
}else if((error>-20)&&(error<20)&&First==0&&sum>3){
//Proportional Signal
propSignal = (error)*Kp;
propSignal=(propSignal/160)*200;
//Integral Signal
sumError += error;
intSignal = (sumError)*Ki;
//trial and error find an x value that works
derSignal = ((error-prevError)/sleepTime)*Kd;
//right wheel
set_motor(1, minSpeed - (propSignal + intSignal + derSignal));
// left wheel
set_motor(2, minSpeed + (propSignal + intSignal + derSignal));
}else if(((errorL)>errorR+1)&&First==0){
//turns90 left
set_motor(2, 70);
set_motor(1, -40);
} else if(((errorL+1)<errorR)&&First==0){
//turns90 right
set_motor(2, -40);
set_motor(1, 70);
}else if(First==0){
//finds the line again
set_motor(1,-60);
set_motor(2, 60);
}
}
while(1){
double kp=0.6;
double kd=0.01;
int right=read_analog(2);
int left=read_analog(4);
error=right-left;
propSignal=error*kp;
propSignal=((propSignal/600)*100);
derSignal=(((error-prevError)/sleepTime)*kd);
prevError=error;
if (front>300){
set_motor(1,-50);
set_motor(2,70);
}else{
set_motor(1, minSpeed - (propSignal + intSignal + derSignal));
// left wheel
set_motor(2, minSpeed + (propSignal + intSignal + derSignal));
}
}
return 0;
}
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;
}
