-
Notifications
You must be signed in to change notification settings - Fork 0
/
temp.cpp
138 lines (122 loc) · 3.21 KB
/
temp.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
#include <stdio.h>
//#include <pthread.h>
#include <time.h>
#include <string.h>
// sudo gcc -Wall
extern "C" int init_hardware();
extern "C" int init(int d_lev);
extern "C" int take_picture();
extern "C" char get_pixel(int row, int col, int color);
extern "C" void set_pixel(int col, int row, char red,char green,char blue);
extern "C" int open_screen_stream();
extern "C" int close_screen_stream();
extern "C" int update_screen();
//extern "C" void GetLine(int row,int threshold);
extern "C" int display_picture(int delay_sec,int delay_usec);
extern "C" int save_picture(char filename[5]);
extern "C" int set_motor(int motor,int speed);
extern "C" int read_analog(int ch_adc);
extern "C" int Sleep(int sec, int usec);
extern "C" int select_IO(int chan, int direct);
extern "C" int write_digital(int chan,char level);
extern "C" int read_digital(int chan);
extern "C" int set_PWM(int chan, int value);
extern "C" int connect_to_server( char server_addr[15],int port);
extern "C" int send_to_server(char message[24]);
extern "C" int receive_from_server(char message[24]);
bool lose_line(){
int white=0;
for(int num=0;num<320;num++){
int value=get_pixel(num, 160, 3);
if(value>110){
white+=1;
if(white>=10){
return false;
}
}else{
white=0;
}
}return true;
}
double line() {
int sum = 0;
double kp = 0.8; //example value, testing needed
int colourVal, s;
int SPEED=80;
double proportional_signal;
take_picture(); // take camera shot
for(int col=0; col < 320; col++){
colourVal=get_pixel(col, 120, 3);
if(colourVal>127){s=1;}//if it's closer to white
else if(colourVal<127){
set_motor(2,-1*SPEED);//right motor
set_motor(1,-1*SPEED);//left motor
Sleep(1,0);
}
sum = sum + (col-160)*s;
}
update_screen();
proportional_signal = sum*kp;
return proportional_signal;
}
double speedCheck(int min, int max, double val){
if(val<min){
val = min;
}
if(val>max){
val = max;
}
return val;
}
int motorControl(double error_signal)
{
printf(" error signal: %f\n",error_signal);
int SPEED = 80;
double modSpeed;
//if too far left
if(error_signal < 0){
modSpeed = speedCheck(0, SPEED, SPEED-error_signal);
set_motor(2,SPEED);//right motor
set_motor(1,SPEED/2);//left motor
printf("Too far left!\n");
printf("Left motor: %d Right motor %d\n",SPEED, modSpeed);
}
//if too far right
else if(error_signal > 0){
modSpeed = speedCheck(0, SPEED, SPEED-error_signal);
set_motor(2,SPEED/2);
set_motor(1,SPEED);
printf("Too far right!\n");
printf("Left motor: %d Right motor %d\n", modSpeed, SPEED);
}
//if centered
else{
set_motor(1,SPEED);
set_motor(2,SPEED);
printf("Going straight\n");
}
return 0;
}
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;
}