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];
}
}
}
}
//CODE for just Proportional Signal
// - This code takes multiple parameters
// - y is which line to read (robot only moves if y is 160)
// - pkp is just the proportional constant that can be changed (As quadrant three has sharp turns, this number needs to change often)
// - base_speed is just to balance with the pkp changes
// - threshChange is for quadrant four (we attempted to write code that can do quadrant 4 without IR Sensors hahaha)
// This method returns whiteC
int p(int y, double pkp, int base_speed, int threshChange){
int p = 0;
int whiteC2 = 0;
int whiteC = 0;
take_picture();
for (int i = 0; i < 320; i++){
if (get_pixel(i,y,3)>THRESH+threshChange){
p += i-160;
whiteC += 1;
}
}
for (int i = 0; i <320; i++){
if (get_pixel(i,80,3)>THRESH+threshChange){
whiteC2 += 1;
}
}
p = p*pkp/160;
if (y == 160){
if (whiteC == 0){
move(BASE_BACK_SPEED,BASE_BACK_SPEED);
Sleep(0,300000);
} else {
move(base_speed+p,base_speed-p);
}
}
return whiteC;
}
//Turns depending on the direction put
// it should turn forever until a line is detected
void turn(int dir) {
move(40,40);
Sleep(0,300000);
int left;
int right;
if (dir == 1){
left = -60;
right = 60;
printf("Moving left\n");
} else if (dir == 2){
left = 60;
right = -60;
printf("Moving right\n");
}
int whiteC = 0;
while (whiteC < 80){
take_picture();
whiteC = 0;
for (int i = 100; i < 220; i++){
if (get_pixel(i,80,3) > 127){
whiteC++;
}
}
move(left,right);
}
move(0,0);
Sleep(0,300000);
}
int init(){
const int self_node_id = 2;
/*
* Node initialization.
* Node ID and name are required; otherwise, the node will refuse to start.
* Version info is optional.
*/
auto& node = getNode();
node.setNodeID(self_node_id);
node.setName("org.kmti.cam.control");
/*
* Start the node.
* All returnable error codes are listed in the header file uavcan/error.hpp.
*/
const int node_start_res = node.start();
if (node_start_res < 0) {
printf("Node init: node failed to start with error %d", node_start_res);
return node_start_res;
}
uavcan::Subscriber<uavcan::equipment::camera::CameraCommand> camera_sub(node);
const int camera_sub_start_res =
camera_sub.start(
[&](const uavcan::ReceivedDataStructure<uavcan::equipment::camera::CameraCommand>& msg)
{
printf("Yay, we got a message!");
switch(msg.command_type) {
case msg.COMMAND_TAKE_PICTURE:
take_picture();
break;
case msg.COMMAND_TURN_ON:
turn_on_camera();
break;
case msg.COMMAND_TURN_OFF:
turn_off_camera();
break;
}
});
if (camera_sub_start_res < 0) {
printf("Camera subscriber: subscriber failed to start with error %d", camera_sub_start_res);
return camera_sub_start_res;
}
/*
* Informing other nodes that we're ready to work.
* Default mode is INITIALIZING.
*/
node.setModeOperational();
node_thread.start(NORMALPRIO);
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 cam_error(){
//Initialises variables for finding colour of pixel, the running total, and the number of white pixels for averaging
long total = 0;
int white = 0;
int pixel;
//Takes picture to analyse
take_picture();
for(int y=115;y<125;y++){
for(int x=0;x<320;x++){
//Read pixel
pixel = get_pixel(x,y,3);
//If pixel is white, add to total
if(pixel>128){
total = total + (x-160);
white++;
};
};
};
//Find average if white pixels were present
if(white>0){
total=total/white;
};
//Print total
printf("%s", "Base error value: ");
printf("%d\n",total);
//Perform PID
int pid_sum; //Declares sum error variable
int int_error; //Declares integral error variable
int prop_error = total*kp; //Find proportional error
int der_error = ((prop_error-prev_error)/0.2)*kd; //Find derivative error (assume camera check is every 2 seconds)
prev_error = prop_error; //Update previous error for next iteration
total_error = total_error + prop_error; //Update total error for integration
int_error = total_error*ki; //Find integration error
pid_sum = prop_error+der_error+int_error; //Find sum error
printf("%s", "Sum error value: ");
printf("%d\n",pid_sum);
printf("%s", "Potential error value: ");
printf("%d\n",prop_error);
printf("%s", "Derivative error value: ");
printf("%d\n",der_error);
printf("%s", "Integration error value: ");
printf("%d\n",int_error);
printf("%s", "Previous error value: ");
printf("%d\n",prev_error);
printf("%s", "Total error value: ");
printf("%d\n\n",total_error);
return pid_sum;}
int getLineValue() {
char c;
//the first array getting the camera input
int whiteness[320];
//whether the number is white enough
int white_thr = 127;
// the second array to compare to (- on left, + on the right)
int white_compare[320];
// what should be the multiplied version of the two above arrays
int white_final[320];
//the sum of all the white final numbers added
int sum = 0;
// a count of how many white spots have been counted( to check if we can still see the line)
int n_whites = 0;
// left motor vroom vroom
int leftMotor = 0;
// right motor also vroom vroom
int rightMotor = 0;
float kp = 0.005;
double proportional_signal =0;
take_picture();
for (int i = 0; i < 320; i++) {
//Take picture with camera
//get pixel "whiteness" from centre of image
c = get_pixel(i, 120, 3);
//printf("%d\n", c);
if (c > white_thr) {
// if its greater than the threshold, the number is a one, and therefore white, so count increases
whiteness[i] = 1;
n_whites++;
}
else {
whiteness[i] = 0;
}
white_compare[i] = i - 160;
white_final[i] = whiteness[i] * white_compare[i];
}
// what should be the sum of white final working
for (int i = 0; i < 320; i++) {
sum = sum + white_final[i];
}
proportional_signal = sum*kp
printf("%d\n", sum);
return proportional_signal;
}
int main(){
//This sets up the RPi hardware and ensures
//everything is working correctly
init(1);
char c;
while(true){
//Take picture with camera
take_picture();
//get pixel "whiteness" from centre of image (160x120)
c = get_pixel(160,120,3);
//Prints read pixel value
printf("%d\n",c);
//Waits for 0.5 seconds (500000 microseconds)
Sleep(0,500000);
}
return 0;}
/**
* Samples a snapshot from the camera to retrieve the line following error
*
* @param white_count The number of white pixels detected, updated by reference.
* @return The line-following error in the image
*/
inline int sample_image(LineInfo &line)
{
line = {false, false, false, false, 0, 0};
int error = 0;
take_picture();
// Get pixels from across the entire image, not just one line
for (int x = 0; x < IMAGE_SIZE_X; x += SAMPLE_STEPS) {
for (int y = 0; y < IMAGE_SIZE_Y; y += SAMPLE_STEPS)
{
error += sample_pixel(line, x, y);
}
}
return error;
}
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;
}
void Quad3Process()
{
take_picture();
n_whites_mid = 0;
for (int i = 0; i < NTP; i++) {
int x = dx * i + (dx / 2);
midPic[i] = get_pixel(x, 1, 3);
if (midPic[i] > THRESHOLD) {
// 1 = white
midPic[i] = 1;
n_whites_mid++;
}
else {
// 0 = black
midPic[i] = 0;
}
}
}
/* update; triggers by distance moved
*/
void AP_Camera::update()
{
if (AP::gps().status() < AP_GPS::GPS_OK_FIX_3D) {
return;
}
if (is_zero(_trigg_dist)) {
return;
}
if (_last_location.lat == 0 && _last_location.lng == 0) {
_last_location = current_loc;
return;
}
if (_last_location.lat == current_loc.lat && _last_location.lng == current_loc.lng) {
// we haven't moved - this can happen as update() may
// be called without a new GPS fix
return;
}
if (get_distance(current_loc, _last_location) < _trigg_dist) {
return;
}
if (_max_roll > 0 && fabsf(ahrs.roll_sensor*1e-2f) > _max_roll) {
return;
}
if (_is_in_auto_mode != true && _auto_mode_only != 0) {
return;
}
uint32_t tnow = AP_HAL::millis();
if (tnow - _last_photo_time < (unsigned) _min_interval) {
return;
}
take_picture();
_last_location = current_loc;
_last_photo_time = tnow;
}
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;
}
int main (){
// This sets up the RPi hardware and ensures
// everything is working correctly
init(0);
// int arrayOfPixels[64]; //For every 5 pixels (340/5 = 64)
// int total = 0;
float kd = 1.2;
// float ki = 0.5;
// float kd = 0.2;
// float total_error = 0;
float current_error = 0;
// int previous_error = 0;
// int error_diff = 0;
// int error_period = 0; //need to change
float pid;
int w, s;
double proportional_signal;
// int integral_signal = 0;
// int derivative_signal = 0;
// for(i = 0; i < 8; i++){
// select_IO(i, 0);
// write_digital(i, 1);
// }
while (1) {
take_picture();
int color = get_pixel(102, 55, 3);
// printf("Color=%d\n", color);
// int adc_reading = 0;
current_error = 0;
proportional_signal = 0;
// for(i = 0; i < 64; i++){
//need to check if 120 is the middle or not
// arrayOfPixels[i] = get_pixel(120,i*5, 3);
//Getting total of the pixels along the pane
// total = total + arrayOfPixels[i];
// }
for(int i=0; i<320; i++){
w = get_pixel(i,120,3);
// printf("%d\n",w);
if(w>120){
s = 1;
}else{
s = 0;
}
//if to right, positive
current_error = (current_error + (i-160)*s);
// printf("current_error is %f \n", current_error);
}
// printf("PID: %f \n", pid);
//Random error checking code, most of which isn't implemented yet
// total_error = total_error + current_error;
// integral_signal = total*ki;
// error_diff = current_error-previous_error;
// derivative_signal = (error_diff/error_period)*kd;
// previous_error = current_error;
//Getting the robot to move
proportional_signal = current_error*kd;
// printf("Current Error: %f \n", current_error);
// printf("Proportional Signal %f \n", proportional_signal);
/** int abc;
if(proportional_signal > 0){
abc = 1;
}else if(proportional_signal < 0){
abc = -1;
}else{
abc = 1;
}*/
pid =( proportional_signal );
printf("PID: %f \n", pid);
if(pid > 255){
set_motor(1,-1*255);
set_motor(2, 255);
}else if(pid < -255){
set_motor(1, 255);
set_motor(2,- 1*255);
}else{
set_motor(1, -pid);
set_motor(2, -pid);
}
}
return 0;
}
int main(){
// Sets up raspbery pi hardware and ensures everything is working.
init(0);
std::signal(SIGINT, handle_signal);
openGate();
// Test code for camera, takes picture and prints it.
int black_count = 0;
int counter = 0;
bool first_full_line = true;
//maze();
while(true){
// Reads current image from camera stores in memory.
take_picture();
int total=0;
int total_error = 0;
int num_white = 0;
int num_col_white = 0;
int total_white = 0;
int prev_error = 0;
bool c;
// loop takes a line of pixels 5 wide horizontally accross the picture
for(int i=0; i<PICTURE_WIDTH; i++){
for(int j = 0; j<5; j++){
// c gets assigned 0 or 1
c = get_pixel(i, (PICTURE_HEIGHT/2)+j, 3) > 127;
// 0 or 1 added to total_white
total_white += c;
}
// num_white gets the values from just 1 pixel line across the picture.
c = get_pixel(i, PICTURE_HEIGHT/2, 3) > 127;
num_white += c;
// added to the total
total += (i-(PICTURE_WIDTH/2))*c;
}
total_error += total;
// PID control see https://github.com/kaiwhata/ENGR101-2016/wiki/PID-(Proportional-Integral-Derivative)-Control
// for more detail
double proportional_signal = total*KP;
double derivative_signal = (total-prev_error/0.1)*KD;
double integral_signal = total_error*KI;
prev_error = total;
int total_signal = proportional_signal + derivative_signal + integral_signal;
//deadend checker
counter++;
// if the camera sees nothing but black implements black_count
if(num_white == 0){
black_count++;
}
// takes a picture of a verticle column in centre of picture
// currently unused.
for(int i=0; i<PICTURE_HEIGHT; i++){
c = get_pixel(PICTURE_WIDTH/2, i, 3) > 127;
num_col_white += c;
}
// detecting if picture is fully white, e.g. only see white
if (num_white >= 320){
printf("hit\n");
if(first_full_line == true){
// drives straight through
first_full_line = false;
set_motor(1, MOTOR_SPEED);
set_motor(2, -MOTOR_SPEED);
Sleep(1,0);
} else {
//turns left
set_motor(1, -MOTOR_SPEED - total_signal);
set_motor(2, -MOTOR_SPEED - total_signal);
}
} else if(num_white > 0){
// drives forward
set_motor(1, MOTOR_SPEED - total_signal);
set_motor(2, -MOTOR_SPEED - total_signal);
} else {
// turns
set_motor(1, -MOTOR_SPEED);
set_motor(2, MOTOR_SPEED);
}
// if stuck at dead end for too long spins around
if(black_count == 25){
black_count = 0;
set_motor(1, MOTOR_SPEED);
//turns avc around
set_motor(2, MOTOR_SPEED);
Sleep(1,0);
}
//resetes counting how many times it detects dead end.
if(counter == 60){
counter = 0;
black_count =0;
}
/*printf("num white:%d\n", num_white);
printf("black:%d\n", black_count);
printf("Counter:%d\n", counter);
printf("Row:%d\n", num_white);
printf("Col:%d\n", num_col_white);*/
// detecting if at red square for start of maze
int num_red = 0;
for(int i=0; i<PICTURE_WIDTH; i++){
bool r = get_pixel(i, PICTURE_WIDTH/2, 0) > 200;
bool g = get_pixel(i, PICTURE_WIDTH/2, 1) > 200;
bool b = get_pixel(i, PICTURE_WIDTH/2, 2) > 200;
if(r && !g && !b){
num_red++;
}
}
//printf("Red:%d\n", num_red);
if(num_red > 100){
set_motor(1, 0);
set_motor(2, 0);
maze();
}
Sleep(0,SLEEP_TIME);
}
return 0;
}
int move(){
int mtrSp = 50;
int check = 0;
int white_threshold = 130;//Threshold of white, i.e. from the 0 to 255 only values above this are detected
float kP = 0.2;//Prop constant which scales with error signal
float kD = 0.00015;
int pastError = 0;//Past error to work out kD
int currentError = 0;//Absolute of error signal - will need to check that works
int eValue = 0;//Average value of error either side
//Sleep(2,0); //For gate
while(1){
int totalSum = 0;
int pSignal = 0;
int dSignal = 0;
int sum = 0;
int num = 0;
int eValue = 0;
int w = 0;
int left = 0; //True if line is left
int right = 0; //True if line is right
int top = 0; //True if line is forward
int leftSum = 0; //Totals amount of left mid pixels which are white
int rightSum = 0; //Totals amount of right mid pixels which are white
int topSum = 0; //Totals amount of top mid pixels which are white
//Totals amount of top mid pixels which are white
take_picture();
for(int i = 0; i < 200; i++){ //For loop to save pixels to arrays and test whiteness, iterates through from a base value to reach a max
//For left and right, this is from row 100 to row 215, column 1 and 319 respectively
//For top this is from
int leftSide = get_pixel(40, i, 3);//Saves the value of the left-mid pixels if above threshold
if(leftSide > 130){
leftSum = leftSum + 1;//Adds to a total count of pixels that are white
}else{//If not valid pixel skip
leftSum = leftSum + 0;
}
int rightSide = get_pixel(280, i, 3);//Saves the value of the right-mid pixels if above threshold
if(rightSide > 130){
rightSum = rightSum + 1;//Adds to a total count of pixels that are white
}else{//If not valid pixel skip
rightSum = rightSum + 0;
}
}
if(leftSum > 40){
left = 1;
}else{
left = 0;
}
if(rightSum > 40){
right = 1;
}else{
right = 0;
}
if(topSum > 20){
top = 1;
}else{
top = 0;
}
for(int i = 0; i < 320; i++){ /**Less than 320 as the image is 320 pixels across*/
sum = get_pixel(i, 1, 3);//Gets pixel at row 1 as it goes from 1 to 240
if(sum > 130){ //If value greater than threshold make it 1 and add to num
w = 1;
num++; //num increases when a white pixel is found
topSum = topSum + 1;
}else{
w = 0;
}
totalSum = totalSum + ((i - 160) * w);//Takes the position of the i and adds to a total
}
printf("num %d\n", num);
if(num > 310){
int check = check + 1;
printf("Check %d\n", check);
//mtrSp = 40;
if(check > 6){
if(left == 1 && right == 1){ //T intersection (choose left)
set_motor(1, 0);
set_motor(2, -55);
Sleep(0, 500000);
//set_motor(1, 0);
//set_motor(2, 0);
}else if(left == 0 && right == 1){ //Right side turn
printf("Right %d\n", right);
set_motor(1, 55);
set_motor(2,0);
Sleep(0, 500000);
//set_motor(2,0);
//set_motor(1, 0);
}else if(num < 20){ //If not enough pixels are found, reverse and reset
set_motor(1, -40.5);
set_motor(2, 40);
Sleep(0, 50000);
continue;
}
}
}else if(num < 20){ //If not enough pixels are found, reverse and reset
set_motor(1, -40.5);
set_motor(2, 40);
Sleep(0, 50000);
continue;
}else{
eValue = totalSum/num;//Finds average of a point at
pSignal = eValue*kP;//Times it by kP to get a value scaled with the e sginal
currentError = abs(eValue);
dSignal = abs(((currentError - pastError)/0.005)*kD);
pastError = currentError;
if(pSignal > 0){/**right*/
//printf("right %d\n", pSignal);
set_motor(1, mtrSp);
if(-(mtrSp + 0.5) + pSignal + dSignal <= 0){
set_motor(2, -(mtrSp + 0.5) + pSignal + dSignal);
}else{
set_motor(2, 0);
}
Sleep(0, 5000);
}else if(pSignal < 0){/**left*/
//printf("left %d\n", pSignal);
if(mtrSp + pSignal + dSignal >= 0){
set_motor(1, mtrSp + pSignal + dSignal);/**From a few calculations 40 seems roughly right, max value is 70ish*/
}else{
set_motor(1, 0);
}
set_motor(2, -(mtrSp + 0.5));/**Minuses values if signal is minus it is double negative therefore positive*/
Sleep(0, 5000);
}
}
}
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);
}
}
//called every 1 ms
interrupt void WDT_interval_handler(){
int leftDist = get_latest_left();
int rightDist = get_latest_right();
int frontDist = get_latest_front();
bool iAmStuck = false;
sensor_conversions_side--;
sensor_conversions_front--;
stuckCounter--;
if (camera_timeout == 1) {
camera_timeout = 0;
set_camera_gpio(false);
} else if (camera_timeout > 0) {
camera_timeout--;
}
//trigger the front sensor every 100ms
if (sensor_conversions_side == 0) {
sensor_conversions_side = SENSOR_LOOPS_SIDE;
ADC10CTL0 |= ADC10SC; // trigger a conversion
}
//trigger an ADC conversion on left and right sensors every 20ms
if (sensor_conversions_front == 0) {
sensor_conversions_front = SENSOR_LOOPS_FRONT;
make_front_measurement();
}
//keep track of the last 18 sensor measurements
if (stuckCounter == 0) {
stuckCounter = STUCK_COUNT;
//keep track of the current sensor values
char current = idx%20;
char prev = (idx-1)%20;
lastData[current][0] = abs(lastData[prev][0] - rightDist);
lastData[current][1] = abs(lastData[prev][1] - leftDist);
lastData[current][2] = abs(lastData[prev][2] - frontDist);
idx++;
//if the car hasnt moved in the last 18 sensor measurements
iAmStuck = amIStuck();
}
//if the car isnt stopped, take a picture every 1000ms
if (ps != STOPPED) {
if (pictureTimer <= 0) {
pictureTimer = PICTURE_TIMER;
bool success = take_picture();
}
}
//automated driving logic
pictureTimer--;
switch (ps) {
case STOPPED:
//start moving, just go for it
forward(defaultPWM);
straight();
ps = F_STRAIGHT;
break;
case F_STRAIGHT:
//stuck, car hasnt moved in 3 seconds
if (iAmStuck) {
reverse(defaultPWM);
straight();
ps = R_STRAIGHT;
turningAround = 1;
turnAroundCounter = TURNAROUND_LENGTH;
init_lastData();
iAmStuck = false;
break;
}
//up against a wall
if (frontDist <= FRONT_THRESHOLD) {
reverse(defaultPWM);
straight();
ps = R_STRAIGHT;
turningAround = 1;
turnAroundCounter = TURNAROUND_LENGTH;
}
//looking down and open hallway
else if (leftDist > BIG_THRESHOLD) {
left();
ps = F_LEFT;
}
else if (rightDist > BIG_THRESHOLD) {
right();
ps = F_RIGHT;
}
//off center in a hallway
else if (leftDist - rightDist > TURN_THRESHOLD) {
left();
ps = F_LEFT;
}
else if (rightDist - leftDist > TURN_THRESHOLD) {
right();
ps = F_RIGHT;
}
break;
case F_LEFT:
last_turn = F_LEFT;
if (iAmStuck) {
reverse(defaultPWM);
straight();
ps = R_STRAIGHT;
turningAround = 1;
turnAroundCounter = TURNAROUND_LENGTH;
init_lastData();
iAmStuck = false;
break;
}
//in the process of turning around
if (turningAround == 1) {
turnAroundCounter--;
if (turnAroundCounter <= TURNAROUND_LENGTH/2) {
forward(defaultPWM);
straight();
turnAroundCounter = 0;
ps = F_STRAIGHT;
turningAround = 0;
}
}
//turning back to center wheels
else if (turningBack == 1) {
turnBackCounter--;
if (turnBackCounter <= 0) {
straight();
ps = F_STRAIGHT;
turnBackCounter = 0;
turningBack = 0;
}
}
//stop turning, we are now centered in hallway
else if (rightDist >= leftDist) {
right();
ps = F_RIGHT;
turningBack = 1;
turnBackCounter = TURNBACK_LENGTH;
}
break;
case F_RIGHT:
last_turn = F_RIGHT;
if (iAmStuck) {
reverse(defaultPWM);
straight();
ps = R_STRAIGHT;
turningAround = 1;
turnAroundCounter = TURNAROUND_LENGTH;
init_lastData();
iAmStuck = false;
break;
}
if (turningAround == 1) {
turnAroundCounter--;
if (turnAroundCounter <= TURNAROUND_LENGTH/2) {
forward(defaultPWM);
straight();
turnAroundCounter = 0;
ps = F_STRAIGHT;
turningAround = 0;
}
}
else if (turningBack == 1) {
turnBackCounter--;
if (turnBackCounter <= 0) {
straight();
ps = F_STRAIGHT;
turnBackCounter = 0;
turningBack = 0;
}
}
else if (leftDist >= rightDist) {
left();
ps = F_LEFT;
turningBack = 1;
turnBackCounter = TURNBACK_LENGTH;
}
break;
case R_STRAIGHT:
turnAroundCounter--;
if (frontDist >= 3*FRONT_THRESHOLD || turnAroundCounter <= 0) {
turnAroundCounter = TURNAROUND_LENGTH;
if (last_turn == F_RIGHT) {
ps = R_RIGHT;
right();
}
else {
ps = R_LEFT;
left();
}
}
break;
case R_RIGHT:
if (turningAround == 1) {
turnAroundCounter--;
if (turnAroundCounter <= 0) {
forward(defaultPWM);
left();
turnAroundCounter = TURNAROUND_LENGTH;
ps = F_LEFT;
}
}
break;
case R_LEFT:
if (turningAround == 1) {
turnAroundCounter--;
if (turnAroundCounter <= 0) {
forward(defaultPWM);
right();
turnAroundCounter = TURNAROUND_LENGTH;
ps = F_RIGHT;
}
}
break;
default:
stop();
straight();
ps = STOPPED;
break;
}
}
int main (){
init_hardware();
// init(0);
int brightnessArray[WIDTH];
int threshold = 120;
int generatedArray[WIDTH];
int errorArray[WIDTH];
int lagErrorArray[WIDTH];
int baseSpeed = 35;
int leftSpeed=baseSpeed;
int rightSpeed=baseSpeed;
bool check=true;
int countrev=0;
int previousError = 0;
int derivative = 0.0;
double kd = 0.00000005;
bool checkintersection=false;
int intcount=0;
int sumError=0;
int incount=0;
bool right90=true;
bool left90=true;
int stuckCount = 0;
//network
bool gate = true;
char ip[14] = "130.195.6.196";
char message[7];
int ir_sensor = read_analog(0);
//connect_to_server(ip,1024);
double k=0.0008; // constant
// an array starting from -160 to 160
int number = -160;
for(int y=0; y<WIDTH+1; y++){
generatedArray[y] = number;
number++;
}
int counter = 0;
int zeroCount = 0;
while(true){
take_picture();
ir_sensor=read_analog(0);
//display_picture(0.5,0);
//open_screen_stream();
//update_screen();
previousError = sumError;
sumError=0;
for(int i=0; i<WIDTH; i++){
char colour = get_pixel(i, HEIGHT/2, 3);
if(colour > threshold){
brightnessArray[i] = 1;
} else {
brightnessArray[i] = 0;
}
errorArray[i] = brightnessArray[i] * generatedArray[i];
sumError += errorArray[i];
}
derivative = ((sumError-previousError)/0.000025);//.000025
printf("Sum error: %d\n",sumError);
printf("Previous error: %d\n",previousError);
//90 DEGREE TURNS
//Right and left 90 Degree turn
//int nine=0;
/*
int turnCounter = 0;
if(sumError>10000){ //sum should be large as the right hand side(0-160) will be white
right90=true;
}else{
right90=false;
}
if(sumError<-10000){ //sum should be very small as the left hand side(0 to -160) will be white
left90=true;
}else{
left90=false;
}
if(right90){
while(turnCounter<25){
set_motor(0,baseSpeed);
set_motor(1,-baseSpeed);
turnCounter++;
}
}
if(left90){
while(sumError<25){
set_motor(0,-baseSpeed);
set_motor(1,baseSpeed);
turnCounter++;
}
}
turnCounter=0;
*/
// calculate speed for left and right motors k has to be small like 0.1
//INTERSECTION CODE
//for(int in=0;in<WIDTh;in++){
//if((birghtnessArray[h])==1){ //Goes through array and because the camera should read all white the sum should have very few 0's
//checkintersection=true; //if it counts 4 blacks then its not an intersection
//}
//else if{intcount<4){
//checkintersection=true;
//incount++
//}
//else if(intcount==4){
//checkintersection=false;
//break;
//}
//}
//if(checkintersection){
//set_motor(0,45)
//set_motor(1,45)
//}
//}//~~~~~unsure what this is for/what it closes off
for(int h=0;h<WIDTH;h++){
if((brightnessArray[h])==0){
check=false;
}else if(countrev<4){
check=false;//this used to be true, changed it to false in case of the last error vlaue.
countrev++;
}else if (countrev==4){
check=true;
break;
}/*
if ((brightnessArray[h])==1){
checkintersection=true;
}else if(intcount<4){
checkintersection=true;
incount++;
}else if(intcount==4){
checkintersection=false;
break;
}*/
//continue;
}
/*for(int i=0; i<WIDTH;i++){
if(brightnessArray[i]==1){
checkintersection=true;
}else if(intcount<1){
checkintersection = true;
incount++;
}else if(intcount==1){
checkintersection=false;
break;
}
}*/
if(check==true){ // && checkintersection==false){
// printf("Kd*d: %i\n",(kd*derivative));
leftSpeed = baseSpeed + (int)(((k*sumError)-(kd*derivative))); // sumError should be 0 for the robot to go in a straight line
rightSpeed = baseSpeed - (int)(((k*sumError)+(kd*derivative)));
printf("K*sum: %d\n",(k*sumError));
printf("Derivative: %d\n",derivative);
/* if(leftSpeed<baseSpeed){
leftSpeed = 0
} else if(rightSpeed<baseSpeed){
rightSpeed = 0;
}
*/
if(leftSpeed > 255){
leftSpeed = 255;
}
else if(leftSpeed < -255){
leftSpeed = -255;
}
if(rightSpeed > 255){
rightSpeed = 255;
} else if(leftSpeed < -255){
rightSpeed = -255;
}/*
if(sumError>6000 || sumError<-6000){
set_motor(1, baseSpeed);
set_motor(2, baseSpeed);
}*/
if(sumError==0 && previousError<0){
leftSpeed = 0;
rightSpeed = baseSpeed;
}
else if(sumError==0 && previousError>0){
leftSpeed = baseSpeed;
rightSpeed = 0;
}/*else if(sumError==0 && previousError==0){
set_motor(1, baseSpeed);
set_motor(2,-baseSpeed);
}*/
else {
set_motor(1, leftSpeed); // Motor 1 is left motor//left
set_motor(2, rightSpeed);//right
}
printf("Left Speed = %d, Right Speed =%d\n", leftSpeed, rightSpeed);
printf("Sum error: %d\n",sumError);
Sleep(0, 25000); // sleeps for 50 ms
counter++;
if(counter==1000){
set_motor(1, leftSpeed); // Motor 1 is left motor
set_motor(2, rightSpeed);
}
} else if(check==false){// && checkintersection==false) {
//int countback=0;
//while(countback<4){
set_motor(1,-baseSpeed);
set_motor(2,-baseSpeed);
//Sleep(0,5000000);
//countback++;
//}
}/* else if(check==true){
set_motor(1,baseSpeed);
set_motor(2,baseSpeed);
Sleep(3,0);
set_motor(1,0);
set_motor(2, baseSpeed);
}*/
if(gate==true && ir_sensor>200){
// printf("%d", ir_sensor);
set_motor(1, 0);
set_motor(2, 0);
connect_to_server(ip, 1024);
//if(ir_sensor > 1){
send_to_server("Please");
receive_from_server(message);
send_to_server(message);
//gate = true;
//}
gate=false;
Sleep(2,0);
//might be buggy and need to double check
//printf("%s", message);
}
}
return 0;
}
int main(){
//VARIABLE INITIALIZATION
//Networking
char message[24];
//Line Following
char c;
float kp = 0.80;
float ki = 0.02;
float kd = 0.04;
int i;
int leftCheck;
int frontCheck;
int rightCheck;
bool left, front, right;
int whiteTotal, prevWhiteLocation, whiteLocation;
double whiteRatio;
double prevRatio;
double derivWhite;
double integWhite;
//Maze
int leftSensor, rightSensor;
int whiteWall;
bool noLeftWall, noRightWall, noWallAhead;
int THRESHOLD = 250; //Sensor Threshold
int totalWidth;
//Primary Initialization
init(1);
//Networking Section
//Send Signal to open gate
connect_to_server("130.195.6.196", 1024);
send_to_server("please");
receive_from_server(message);
send_to_server(message);
//Line Following Section
//Loop runs until both sensors sense walls (start of maze)
while((read_analog(0) < THRESHOLD) || (read_analog(1) < THRESHOLD)){
//Set variables
left = false;
front = false;
right = false;
whiteTotal = 0;
leftCheck = 0;
frontCheck = 0;
rightCheck = 0;
//Take readings
take_picture();
for(i = 0; i < 240; i++){
c = get_pixel(40, i, 3);
if(c > 120){
whiteTotal++;
whiteLocation = whiteLocation + (i-120);
}
}
for(i = 60; i < 70; i++){
c = get_pixel(i, 60, 3);
if(c > 120){
leftCheck++;
}
}
for(i = 60; i < 70; i++){
c = get_pixel(i, 180, 3);
if(c > 120){
rightCheck++;
}
}
for(i = 30; i < 210; i++){
c = get_pixel(160, i, 3);
if(c > 120){
frontCheck++;
}
}
if(leftCheck > 5){
left = true;
}
if(frontCheck > 10){
front = true;
}
if(rightCheck > 5){
right = true;
}
if(left){
set_motor(1, 50);
set_motor(2, 0);
Sleep(0, 500000); //Left Sleep
}
else if(front && right){
set_motor(1, 50);
set_motor(2, -50);
Sleep(0, 500000); //Front Sleep
}
else if(right){
set_motor(1, 0);
set_motor(2, -50);
Sleep(0, 500000); //Right Sleep
}
else if(whiteTotal < 1){
set_motor(1, -50);
set_motor(2, 50);
Sleep(0, 100000); //Turn around Sleep
}
else{
derivWhite = ((double)whiteLocation - (double)prevWhiteLocation)/0.01;
integWhite = integWhite + ((double)whiteLocation * 0.01);
whiteLocation = whiteLocation/whiteTotal;
// set_motor(1, ((int) ((-(whiteLocation*40/120)*kp+kd*derivWhite)+40)));
// set_motor(2, -((int) (((whiteLocation*40/120)*kp+kd*derivWhite)+40)));
set_motor(1, ((int)(-( ( (whiteLocation*1/3)*kp) + (derivWhite * kd) + (integWhite * ki) + 40))));
set_motor(2, -((int) (((whiteLocation*40/120)*kp)+(derivWhite * kd) + (integWhite * ki) + 40)));
prevWhiteLocation = whiteLocation;
Sleep(0,1000);
}
}
while(true){
}
return 0;
}
int main(){
init(0);
serverConnect();
Sleep(1, 0);
set_motor(1, 50);
set_motor(2, 50);
Sleep(2, 0);
int initSpeed = 30; // was 30
float kP = 0.01; // was 0.012 / was 0.0115 / was 0.01 / was 0.008
float kD = 0.000; // was 0.006
int prevError = 0;
//int speedRatio = (int)((255-initSpeed)/1360); // This wont give a nice round number but we need an int.
// int kI - not nessecary as of
int threshold = 130;//90; was 120
int D = 0;
while(true){
take_picture();
int P = 0;
// int count = 0;
int blackCheck = 0;
int blackThreshold = 90;
int eTotal = 0;
for(int i = 0; i <= 320; i++){
int value = get_pixel(i, 60, 3);
printf("camera value %d\n", value);
if(value > threshold){
value = 1 * (i - 160);
blackCheck++;
//count++;// 160 being the center line and causing a larger error if the white is detected the furthest away.
} else {
//blackCheck++;
value = 0;
}
P += value;
}
D = P - prevError;
prevError = P;
int dSignal = (int) D*kD;
int pSignal = (int) P*kP;
//printf("dSig: %d\n", dSignal);
//printf("pSig: %d\n", pSignal);
int error = pSignal + dSignal;
if(blackCheck < blackThreshold){
//do a 180 turn
set_motor(1,-1*50);
set_motor(2,-1*50);
//Sleep(0,5000); // was 8000
//
}
//printf("dSignal: %d\n", dSignal);
//printf("pSignal: %d\n", pSignal);
if (error < 0) { //if left of line
set_motor(1, initSpeed + (-1*error)); //left motor
set_motor(2, initSpeed);
//printf("left: %d\n", initSpeed + (-1*error));
//printf("right: %d\n", initSpeed);
Sleep(0,5000);//right motor
} else if (error > 0) { //if right of line
set_motor(1, initSpeed); //left motor
set_motor(2, initSpeed + error);
//printf("right: %d\n", initSpeed + error);
//printf("left: %d\n", initSpeed);
Sleep(0,5000);//right motor
}/* else { //if on the line
set_motor(2, initSpeed); //left motor
set_motor(1, initSpeed); //right motor
}*/
//printf("left motor: %d\n", e);
}
set_motor(1, 0);
set_motor(2, 0);
return 0;
}
int move(){
int check = 0;
int white_threshold = 130;//Threshold of white, i.e. from the 0 to 255 only values above this are detected
int w = 0;
float kP = 0.2;//Prop constant which scales with error signal
float kD = 0.00015;
int pastError = 0;//Past error to work out kD
int currentError = 0;//Absolute of error signal - will need to check that works
Sleep(2,0);
while(1){
int totalSum = 0;
int pSignal = 0;
int dSignal = 0;
int sum = 0;
int num = 0;
int eValue = 0;
int leftSum = 0; //Totals amount of left mid pixels which are white
int rightSum = 0; //Totals amount of right mid pixels which are white
int topSum = 0; //Totals amount of top mid pixels which are white
take_picture();//Takes picture and sets all the variables to 0
for(int i = 0; i < 200; i++){ //For loop to save pixels to arrays and test whiteness, iterates through from a base value to reach a max
int leftSide = get_pixel(40, i, 3);//Saves the value of the left-mid pixels if above threshold
if(leftSide > 130){
leftSum = leftSum + 1;//Adds to a total count of pixels that are white
}else{//If not valid pixel skip
leftSum = leftSum + 0;
}
int rightSide = get_pixel(280, i, 3);//Saves the value of the right-mid pixels if above threshold
if(rightSide > 130){
rightSum = rightSum + 1;//Adds to a total count of pixels that are white
}else{//If not valid pixel skip
rightSum = rightSum + 0;
}
}
printf("TopSum %d\n", topSum);
printf("RightSum %d\n", rightSum);
printf("LeftSum %d\n", leftSum);
for(int i = 0; i < 320; i++){ /**Less than 320 as the image is 320 pixels across*/
sum = get_pixel(i, 1, 3);//Gets pixel at row 1 as it goes from 1 to 240
if(sum > white_threshold){ //If value greater than threshold make it 1 and add to num
w = 1;
num++; //num increases when a white pixel is found
topSum = topSum + 1;
}else{
w = 0;
}
totalSum = totalSum + ((i - 160) * w);//Takes the position of the i and adds to a total
}
if(num > 315){ //True if line is forward
printf("Num %d\n", num);
check = check + 1;
if (check > 20){
//Skip left if theres line forward
if((leftSum > rightSum) && (topSum > 100)){
printf("Skip left");
set_motor(1, 55);
set_motor(2,-55);
Sleep(0, 500000);
}
//Skip right if theres line forward
else if ((leftSum < rightSum) && (topSum > 100)){
printf("Skip right");
set_motor(1, 55);
set_motor(2,-55);
Sleep(0, 500000);
}
//4 way intersect just moves forward
else if((leftSum > 100) && (rightSum > 100) && (topSum > 100)){
printf("4 way");
set_motor(1, 55);
set_motor(2,-55);
Sleep(0, 500000);
}
//T-intersect
else if((leftSum > 100) && (rightSum > 100) && (topSum < 60)){
printf("T-intersect");
set_motor(1, 0);
set_motor(2, -55);
Sleep(0, 500000);
set_motor(1, 0);
set_motor(2, 0);
}
//Turn right
else if((leftSum < rightSum) && (topSum < 70)){
printf("Right");
set_motor(1, 55);
set_motor(2, 0);
Sleep(0, 500000);
set_motor(1, 0);
set_motor(2, 0);
}
//Turn left
else if((leftSum > rightSum) && (topSum < 70)){
printf("Left");
set_motor(1, 0);
set_motor(2, -55);
Sleep(0, 500000);
set_motor(1, 0);
set_motor(2, 0);
}
}
//Reverses
}else if(num < 20){
set_motor(1, -40.5);
set_motor(2, 40);
Sleep(0, 50000);
continue;
//PID
}else{
eValue = totalSum/num;//Finds average of a point at
pSignal = eValue*kP;//Times it by kP to get a value scaled with the e sginal
currentError = abs(eValue);
dSignal = abs(((currentError - pastError)/0.005)*kD);
pastError = currentError;
//Turn right
if(pSignal > 0){
set_motor(1, 40);
if(-40.5 + pSignal + dSignal <= 0){
set_motor(2,-40.5 + pSignal + dSignal);
}else{
set_motor(2,0);
}
Sleep(0, 5000);
//Turn left
}else if(pSignal < 0){
if(40 + pSignal + dSignal >= 0){
set_motor(1, 40 - pSignal - dSignal);
}else{
set_motor(1,0);
}
set_motor(2, -40.5);
Sleep(0, 5000);
}
}
}
return 0;
}
int main()
{
init(0);
signal(SIGINT, terminate);
signal(SIGTERM, terminate);
if (gate_test == 1) {
//connects to server with the ip address 130.195.6.196
connect_to_server("130.195.6.196", 1024);
//sends a message to the connected server
send_to_server("Please"); // "Please is the 'codeword', may change
//receives message from the connected server
char message[24];
receive_from_server(message);
send_to_server(message); // send password back to server to open up gate.
}
Sleep(1, 000000);
speed = 90;
v_left = speed - Tcorrection; // speed of left motor
v_right = speed + Tcorrection; // speed of right motor
// Method for completing quadrant 1
while (method == 1) {
Dconstant = 340;
clock_gettime(CLOCK_REALTIME, &now);
prev_ms = (now.tv_sec * 1000 + (now.tv_nsec + 500000) / 1000000); //convert to milliseconds
take_picture();
n_whites_mid = 0;
for (int i = 0; i < NTP; i++) {
int x = dx * i + (dx / 2);
midPic[i] = get_pixel(x, 1, 3);
if (midPic[i] > THRESHOLD) {
// 1 = white
midPic[i] = 1;
n_whites_mid++;
}
else {
// 0 = black
midPic[i] = 0;
}
}
// reset variables
error_mid = 0.0;
for (int l = 0; l < NTP; l++) {
error_mid = error_mid + (zeroCentre[l] * midPic[l]);
}
if (n_whites_mid != 0 && n_whites_mid != NTP) {
GeneralPID();
}
else if (n_whites_mid == NTP) {
method = 2;
}
else if (n_whites_mid == 0) {
// if loses line, swings quickly to direction of last error of line seen
if (Perror_mid > 0) {
// hard left
v_left = -15;
v_right = 40;
}
else if (Perror_mid < 0) {
// hard right
v_left = 40;
v_right = -15;
}
}
Perror_mid = error_mid;
clock_gettime(CLOCK_REALTIME, &now);
now_ms = (now.tv_sec * 1000 + (now.tv_nsec + 500000) / 1000000); //convert to milliseconds
delta_ms = now_ms - prev_ms;
set_motor(1, v_left);
set_motor(2, v_right);
}
// method for completing quadrant 3
while (method == 2) {
speed = 65;
Pconstant = 0.6;
Dconstant = 375;
clock_gettime(CLOCK_REALTIME, &now);
prev_ms = (now.tv_sec * 1000 + (now.tv_nsec + 500000) / 1000000); //convert to milliseconds
take_picture();
n_whites_mid = 0;
n_whites_left = 0;
n_whites_right = 0;
sensor_right = read_analog(2);
for (int i = 0; i < NTP; i++) {
int x = dx * i + (dx / 2);
int y = dy * i + (dy / 2);
midPic[i] = get_pixel(x, 1, 3);
leftPic[i] = get_pixel(1, y, 3);
rightPic[i] = get_pixel(319, y, 3);
if (midPic[i] > THRESHOLD) {
// 1 = white
midPic[i] = 1;
n_whites_mid++;
}
else {
// 0 = black
midPic[i] = 0;
}
if (leftPic[i] > THRESHOLD) {
// 1 = white
leftPic[i] = 1;
n_whites_left++;
}
else {
// 0 = black
leftPic[i] = 0;
}
if (rightPic[i] > THRESHOLD) {
// 1 = white
rightPic[i] = 1;
n_whites_right++;
}
else {
// 0 = black
rightPic[i] = 0;
}
if (sensor_right > 300) {
method = 3;
}
}
// reset variables
error_mid = 0.0;
for (int l = 0; l < NTP; l++) {
error_mid = error_mid + zeroCentre[l] * midPic[l];
}
// t junction
if (midPic[16] == 0 && n_whites_left != 0 && n_whites_right != 0) {
v_left = speed;
v_right = speed;
set_motor(2, v_right);
set_motor(1, v_left);
Sleep(0, 200000);
while (midPic[16] == 0) {
Quad3Process(); // calls method to take image and process values
v_left = speed;
v_right = -0.7 * speed;
set_motor(2, v_right);
set_motor(1, v_left);
}
}
// left turn
else if (midPic[16] == 0 && n_whites_left != 0 && n_whites_right < 5) {
v_left = speed;
v_right = speed;
set_motor(2, v_right);
set_motor(1, v_left);
Sleep(0, 200000);
while (midPic[16] == 0) {
Quad3Process();
v_left = speed;
v_right = -0.7 * speed;
set_motor(2, v_right);
set_motor(1, v_left);
}
}
//right turn
else if (midPic[16] == 0 && n_whites_left < 5 && n_whites_right != 0) {
v_left = speed;
v_right = speed;
set_motor(2, v_right);
set_motor(1, v_left);
Sleep(0, 200000);
while (midPic[16] == 0) {
Quad3Process();
v_left = speed;
v_right = -0.7 * speed;
set_motor(1, v_right);
set_motor(2, v_left);
}
}
// dead end - should do 180
else if (n_whites_mid == 0 && n_whites_left == 0 && n_whites_right == 0) {
while (midPic[16] == 0) {
Quad3Process();
v_left = -speed;
v_right = speed;
set_motor(1, v_right);
set_motor(2, v_left);
}
}
// pid
else if (n_whites_mid != 0 && n_whites_mid != NTP) {
GeneralPID();
}
else if (n_whites_mid == 0) {
v_left = 45;
v_right = 45;
}
Perror_mid = error_mid;
clock_gettime(CLOCK_REALTIME, &now);
now_ms = (now.tv_sec * 1000 + (now.tv_nsec + 500000) / 1000000); //convert to milliseconds
delta_ms = now_ms - prev_ms;
set_motor(1, v_left);
set_motor(2, v_right);
}
while (method == 3) {
Pconstant = 0.6;
int error;
Perror_mid = error;
speed = 40;
Dconstant = 0.35;
// Loop here:
sensor_left = read_analog(0);
sensor_mid = read_analog(1);
sensor_right = read_analog(2);
error = sensor_right - sensor_left;
proportional = error * Pconstant;
proportional = ((proportional / 770) * 100);
derivative = ((error - Perror_mid) / 0.1) * Dconstant;
if (sensor_mid > 470) {
set_motor(1, -speed);
set_motor(2, speed);
Sleep(0, 350000);
}
set_motor(2, speed - proportional);
set_motor(1, speed + proportional);
}
}
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;
}
void packet_scan(uint8_t *data, uint8_t length)
{
if (length > 0)
{
DLOG("Got message, length: ");
DLOG((int)length);
DLOG("\n\r");
switch (data[0])
{
case MID_TAKE_PICTURE:
if(length == 1) {
DLOG("TAKE_PICTURE message received");
int takePictureImageID;
takePictureImageID = take_picture();
if(takePictureImageID >= 0) {
ILOG("Picture taken with image ID:");
ILOG(takePictureImageID);
ILOG("\n\r");
send_PICTURE_TAKEN_message(takePictureImageID);
DLOG("Sent picture taken message.");
} else {
ILOG("Picture taking failed!");
}
} else {
DLOG("Invalid TAKE_PICTURE message received");
}
break;
case MID_IMAGE_DOWNLOAD_REQUEST:
if(length == 3) {
uint16_t downloadRequestImageID;
downloadRequestImageID = (uint16_t)(data[1] << 8) + (uint16_t)data[2];
DLOG("IMAGE_DOWNLOAD_REQUEST message received with image ID ");
DLOG(downloadRequestImageID);
DLOG("\n\r");
// we need to see if an image with the id specified actually exists
char imgFileName[MAX_FILE_NAME_LENGTH];
snprintf(imgFileName, sizeof(imgFileName), "%s%i%s", filePrefix, downloadRequestImageID, fileExt);
DLOG(imgFileName);
DLOG("\n\r");
if(SD.exists(imgFileName)) {
sdFile.close(); // ensure old file is closed
sdFile = SD.open(imgFileName);
if(sdFile == false) {
ILOG("ERROR: JPEG file failed to open!");
imageSendState.sendingImage = false;
send_IMAGE_DOWNLOAD_INFO_message(0);
} else {
imageSendState.currentPacket = 0;
uint32_t size = sdFile.size();
imageSendState.imageFileSize = size;
DLOG("imageSendState.imageFileSize: ");
DLOG(imageSendState.imageFileSize);
DLOG("\n\r");
if(imageSendState.imageFileSize > 0) {
imageSendState.numPackets = imageSendState.imageFileSize / IMAGE_PACKET_SIZE;
if((imageSendState.imageFileSize % IMAGE_PACKET_SIZE) != 0)
imageSendState.numPackets += 1;
DLOG("Number of packets: ");
DLOG(imageSendState.numPackets);
DLOG("\n\r");
imageSendState.imageFile = sdFile;
imageSendState.sendingImage = true;
send_IMAGE_DOWNLOAD_INFO_message(imageSendState.numPackets);
} else {
ILOG("ERROR: Image file size was zero.");
imageSendState.sendingImage = false;
send_IMAGE_DOWNLOAD_INFO_message(0);
sdFile.close();
}
}
} else {
ILOG("ERROR: JPEG file does not exist!");
// send image download message with number of packets = 0 to represent the fact that the file does not exist
send_IMAGE_DOWNLOAD_INFO_message(0);
}
} else {
DLOG("Invalid IMAGE_DOWNLOAD_REQUEST message received");
}
break;
default: // not a valid message
DLOG("Invalid message, length: ");
DLOG(length);
DLOG("\n\r");
break;
}
}
}
int command_dispatcher(int command_id, char * args, char * result_str)
{
int result = 0;
int arg_param;
switch(command_id)
{
case TS_PREVIEW_START:
result = start_preview();
break;
case TS_PREVIEW_STOP:
result = stop_preview();
break;
case TS_VIDEO_START:
result = start_video();
break;
case TS_VIDEO_STOP:
result = stop_video();
break;
case TS_SNAPSHOT_YUV_PICTURE:
result = take_picture(ACTION_TAKE_YUV_PICTURE);
break;
case TS_SNAPSHOT_JPEG_PICTURE:
result = take_picture(ACTION_TAKE_JPEG_PICTURE);
break;
case TS_SNAPSHOT_RAW_PICTURE:
result = take_raw_picture();
break;
case TS_SNAPSHOT_STOP:
result = testsuite_snapshot_stop();
break;
case TS_SYSTEM_INIT:
result = system_init();
break;
case TS_SYSTEM_DESTROY:
result = system_destroy();
break;
case TS_PRINT_MAXZOOM:
result = print_maxzoom();
break;
case TS_PRINT_ZOOMRATIOS:
result = print_zoomratios();
break;
case TS_ZOOM_INCREASE:
result = zoom_increase(1);
break;
case TS_ZOOM_DECREASE:
result = zoom_decrease(1);
break;
case TS_ZOOM_STEP_INCREASE:
result = zoom_increase(0);
break;
case TS_ZOOM_STEP_DECREASE:
result = zoom_decrease(0);
break;
case TS_CONTRAST_INCREASE:
result = increase_contrast();
break;
case TS_CONTRAST_DECREASE:
result = decrease_contrast();
break;
case TS_SATURATION_INCREASE:
result = increase_saturation();
break;
case TS_SATURATION_DECREASE:
result = decrease_saturation();
break;
case TS_SPECIAL_EFFECT:
result = SpecialEffect();
break;
case TS_BRIGHTNESS_INCREASE:
result = increase_brightness();
break;
case TS_BRIGHTNESS_DECREASE:
result = decrease_brightness();
break;
case TS_EV_INCREASE:
result = increase_EV();
break;
case TS_EV_DECREASE:
result = decrease_EV();
break;
case TS_ANTI_BANDING:
result = set_antibanding();
break;
case TS_SET_WHITE_BALANCE:
result = set_whitebalance();
break;
case TS_AEC_MODE:
result = AEC_mode_change();
break;
case TS_ISO_INCREASE:
result = increase_ISO();
break;
case TS_ISO_DECREASE:
result = decrease_ISO();
break;
case TS_SHARPNESS_INCREASE:
result = increase_sharpness();
break;
case TS_SHARPNESS_DECREASE:
result = decrease_sharpness();
break;
case TS_SET_AUTO_FOCUS:
result = set_auto_focus();
break;
case TS_SET_HJR:
result = set_hjr();
break;
case TS_SET_LENS_SHADING:
result = LensShading();
break;
case TS_SET_LED_MODE:
result = LED_mode_change();
break;
case TS_GET_SHARPNESS_AF:
result = set_sharpness_AF();
break;
case TS_SNAPSHOT_RESOLUTION:
arg_param = atoi(args);
result = snapshot_resolution(arg_param);
break;
case TS_PREVIEW_RESOLUTION:
arg_param = atoi(args);
result = preview_video_resolution (arg_param);
break;
case TS_MOTION_ISO:
result = set_MotionIso();
break;
case TS_TOGGLE_HUE:
result = toggle_hue();
break;
case TS_CANCEL_AUTO_FOCUS:
result = cancel_af();
break;
case TS_GET_AF_STEP:
result = get_af_step();
break;
case TS_SET_AF_STEP:
result = set_af_step();
break;
case TS_ENABLE_AFD:
result = enable_afd();
break;
case TEST_VIDIOC_G_FMT:
result = msm_v4l2_vidioc_g_fmt();
break;
case TEST_VIDIOC_S_FMT:
result = msm_v4l2_vidioc_s_fmt();
break;
case TEST_VIDIOC_CROPCAP:
result = msm_v4l2_vidioc_cropcap();
break;
case TEST_VIDIOC_G_CROP:
result = msm_v4l2_vidioc_g_crop();
break;
case TEST_VIDIOC_S_CROP:
result = msm_v4l2_vidioc_s_crop();
break;
case TEST_VIDIOC_QUERYMENU:
result = msm_v4l2_vidioc_querymenu(args);
break;
case TEST_VIDIOC_QUERYCTRL:
result = msm_v4l2_vidioc_queryctrl(NULL);
break;
case TEST_VIDIOC_S_CTRL:
result = msm_v4l2_vidioc_s_ctrl(args);
break;
case TEST_VIDIOC_G_CTRL:
result = msm_v4l2_vidioc_g_ctrl(args);
break;
default:
break;
}
return result;
}
int main(){
//This sets up the RPi hardware and ensures
//everything is working correctly
//Initializing variables
init(0);
int w;
int s;
double P = 0;
double D = 0;
double kp = 0.31;
double kd = 0.003;
int maxSpeed = 40;
int VL;
int VR;
time_t start; //Sets 0 Point for timer to begin the clock at.
time_t finish; //Sets finishing time
int dif;
int count = 0;
double tim = 0.03;
double avgC = 0;
double current_error = 0;
double previous_error = 0;
double delta_error;
int setTime = 0;
bool line = true;
int iCount = 0;
double lastCurrent_error = 0;
bool white_light;
//------------------------------------------------------------------------------
while(true){
start = time(NULL);//Starts Timer
take_picture();
white_light = true;
//--------------------------------------Analyzing Image-------------------------
for (int i = 0; i < 320; i++){
w = get_pixel(i,120,3);
if(w < 127){ //If pixel is close to black ------ (Reduces noise)
s=0;
}else {
s=1;
white_light = false;
}
current_error = current_error + (i-160)*s; //Adds to current_error if its a white pixel
}//Closes For Loop
//-------------------------------------No Line Detected-------------------------
if(current_error == 0 && white_light){ //error is 0 and black_light
line = false;
VL = maxSpeed;
VR = maxSpeed;
set_motor(1,-VL);//Sets motors to reverse
set_motor(2,VR);
iCount++; //Counts number of frames line has been missing for
}else{
line = true;
iCount = 0;
}
if(iCount>0){ //Debugging
printf("Count = %d (line has been missing for %d FPS)\n",iCount,iCount);
}
if( current_error<0 || current_error>0 ){
lastCurrent_error = current_error;
}
if(iCount>7){ //Line has been lost for a while. Run Correction
if(lastCurrent_error>0){
set_motor(1,-VL);
set_motor(2,-VR);
Sleep(0,100000);
}else if(lastCurrent_error<0){
set_motor(1,VL);
set_motor(2,VR);
Sleep(0,100000);
}
}//Closes iCount if statement
//--------------------------------------Time Stamp & FPS------------------------
finish = time(NULL);
dif = finish -start;
if(dif==1){// Only occurs when 1 second has passed when 1 second has passed.
tim = 1/count; //Updates the current time taken for AVC to process 1 Frame
count = 0;
}
//---------------------------------------Error Values---------------------------
avgC = avgC + (current_error/320);
P = (current_error/320)*kp;
//printf("D = %f\nP = %f\n",D,P); //Debugging
current_error = 0; //Resets current error to 0
setTime++;
count++;
//----------------------------- D value for PID --------------------------------
if(setTime == 1){ //This means our D value changes every frame
D = ((avgC-previous_error)/(0.03))*kd;
previous_error = avgC;
avgC = 0;
setTime = 0;
}
//----------------------------------------Motor Control-------------------------
if(line){
VL = maxSpeed - (P) + (D);
VR = maxSpeed + (P) - (D);
set_motor(1,VL);
set_motor(2,-VR);
}
//-----------------------------------------END OF PROGRAM-----------------------
}//Closes While Loop
return 0;}
int move(){
//Sleep(2,0); //For gate
while(1){
//Totals amount of top mid pixels which are white
take_picture();
for(int i = 0; i < 240; i++){ //For loop to save pixels to arrays and test whiteness, iterates through from a base value to reach a max
//For left and right, this is from row 100 to row 215, column 1 and 319 respectively
//For top this is from
int leftSide = get_pixel(1, i, 3);//Saves the value of the left-mid pixels if above threshold
if(leftSide > 130){
leftSum = leftSum + 1;//Adds to a total count of pixels that are white
}
else{//If not valid pixel skip
leftSum = leftSum + 0;
}
int rightSide = get_pixel(319, i, 3);//Saves the value of the right-mid pixels if above threshold
if(rightSide > 130){
rightSum = rightSum + 1;//Adds to a total count of pixels that are white
}
else{//If not valid pixel skip
rightSum = rightSum + 0;
}
}
for(int i = 0; i < 320; i++){ /**Less than 320 as the image is 320 pixels across*/
sum = get_pixel(i, 1, 3);//Gets pixel at row 1 as it goes from 1 to 240
if(sum > 130){ //If value greater than threshold make it 1 and add to num
w = 1;
num++; //num increases when a white pixel is found
topSum = topSum + 1;
}else{
w = 0;
topSum = topSum + 0;
}
totalSum = totalSum + ((i - 160) * w);//Takes the position of the i and adds to a total
}
if(leftSum > 170){left = 1;}
else{left = 0;}
if(rightSum > 170){right = 1;}
else{right = 0;}
if(topSum > 40){top = 1;}
else{top = 0;}
int Redsum = get_pixel(160, 1, 0);
if(left == 1 && right == 1 && top == 0){
while(Redsum < 150){
printf("Redsum, %d", Redsum);
int Redsum = get_pixel(160, 1, 0);
if(left == 1 && right == 1 && top == 0){ //T intersection (choose left)
printf("T intersect, %d", left);
set_motor(1, 0);
set_motor(2, -60);
Sleep(0, 500000);
set_motor(1, 0);
set_motor(2, 0);
}
else if(left == 0 && right == 1 && top == 0){ //Right side turn
printf("Right");
set_motor(2,0);
set_motor(1, 60);
Sleep(0, 500000);
set_motor(2,0);
set_motor(1, 0);
}
else if(left == 1 && right == 0 && top == 0){ //Left side turn
printf("Left");
set_motor(1, 0);
set_motor(2, -60);
Sleep(0, 500000);
set_motor(1, 0);
set_motor(2, 0);
}
//else if(top == 1){ //Left side turn
else{
//printf("Forward");
take_picture();
for(int i = 0; i < 320; i++){ /**Less than 320 as the image is 320 pixels across*/
sum = get_pixel(i, 1, 3);//Gets pixel at row 1 as it goes from 1 to 240
if(sum > 130){ //If value greater than threshold make it 1 and add to num
w = 1;
num++; //num increases when a white pixel is found
topSum = topSum + 1;
}
else{
w = 0;
topSum = topSum + 0;
}
totalSum = totalSum + ((i - 160) * w);//Takes the position of the i and adds to a total
}
if(num < 20){ //If not enough pixels are found, reverse and reset
set_motor(1, -40.5);
set_motor(2, 40);
Sleep(0, 500000);
continue;
}
else{
eValue = totalSum/num;//Finds average of a point at
pSignal = eValue*kP;//Times it by kP to get a value scaled with the e sginal
currentError = abs(eValue);
dSignal = abs(((currentError - pastError)/0.005)*kD);
pastError = currentError;
if(pSignal > 0){/**right*/
set_motor(1, (35 + pSignal+dSignal));
set_motor(2, -35.5);
Sleep(0, 5000);
}
else if(pSignal < 0){/**left*/
set_motor(1, 35);
set_motor(2, -(35.5 - pSignal + dSignal));
Sleep(0, 5000);
}
}
}
//Totals amount of top mid pixels which are white
take_picture();
for(int i = 0; i < 240; i++){
int leftSide = get_pixel(1, i, 3);//Saves the value of the left-mid pixels if above threshold
if(leftSide > 130){
leftSum = leftSum + 1;//Adds to a total count of pixels that are white
}
else{//If not valid pixel skip
leftSum = leftSum + 0;
}
int rightSide = get_pixel(319, i, 3);//Saves the value of the right-mid pixels if above threshold
if(rightSide > 130){
rightSum = rightSum + 1;//Adds to a total count of pixels that are white
}
else{//If not valid pixel skip
rightSum = rightSum + 0;
}
}
for(int i = 0; i < 320; i++){ /**Less than 320 as the image is 320 pixels across*/
sum = get_pixel(i, 1, 3);//Gets pixel at row 1 as it goes from 1 to 240
if(sum > 130){ //If value greater than threshold make it 1 and add to num
w = 1;
num++; //num increases when a white pixel is found
topSum = topSum + 1;
}
else{
w = 0;
topSum = topSum + 0;
}
totalSum = totalSum + ((i - 160) * w);//Takes the position of the i and adds to a total
}
if(leftSum > 170){
left = 1;
}
else{
left = 0;
}
if(rightSum > 170){
right = 1;
}
else{
right = 0;
}
if(topSum > 40){
top = 1;
}
else{
top = 0;
}
}
}
//else if(num != 0){
else{
if(num < 20){ //If not enough pixels are found, reverse and reset
printf("reverse %d\n", num);
set_motor(1, -40.5);
set_motor(2, 40);
Sleep(0, 50000);
continue;
}
else{
eValue = totalSum/num;//Finds average of a point at
pSignal = eValue*kP;//Times it by kP to get a value scaled with the e sginal
currentError = abs(eValue);
dSignal = abs(((currentError - pastError)/0.005)*kD);
pastError = currentError;
if(pSignal > 0){/**right*/
//printf("right %d\n", pSignal);
set_motor(1, (35 + pSignal+dSignal));
set_motor(2, -35.5);
Sleep(0, 5000);
}else if(pSignal < 0){/**left*/
//printf("left %d\n", pSignal);
set_motor(1, 35);/**From a few calculations 40 seems roughly right, max value is 70ish*/
set_motor(2, -(35.5 - pSignal + dSignal));/**Minuses values if signal is minus it is double negative therefore positive*/
Sleep(0, 5000);
}
}
}
}
return 0;
}
int main(){
//This sets up the RPi hardware and ensures
//everything is working correctly
init(0);
//keep going till line finished or no line
while(true){
//Take picture with camera
take_picture();
preError = avError;
counter = 0;
error = 0;
for (int i=0; i<320; i++){
w = get_pixel(i, 120, 3);
if(w > 100){
error += (i-160);
counter++;
}
}
//rests for 0.1 seconds
Sleep(0,(1000000*sleepTime));
if(counter!=0){
//Proportional Signal
avError = error/counter;
propSignal = (avError/maxP)*Kp;
if(propSignal>Kp){ //ensures the propSignal between 0 - Kp
propSignal = Kp;
}
//Derivative Signal
deltaError = (avError - preError)/(sleepTime);
//x should be the maximum value of deltaError
//trial and error find an x value that works
derSignal = (deltaError/maxD)*Kd;
if(derSignal>Kd){ //ensures the derSignal between 0 - Kd
printf("derSignal: %d", derSignal);
derSignal = Kd;
}
//not sure which wheel is which
//assumes positive is forwards
//should be right wheel
set_motor(1, ((maxSpeed - (Kp + Kd)) - (propSignal + DerSignal)));
//should be left wheel
set_motor(2, ((maxSpeed - (Kp + Kd)) + (propSignal + DerSignal)));
}else{
//turns until line is found.
set_motor(1, -45);
set_motor(2, 40);
}
}
return 0;
}
int main(){
//VARIABLE INITIALIZATION
//Networking
char message[24];
//Line Following
char c;
float kp = 0.80;
float ki = 0.0;
float kd = 0.0;
int i;
int leftCheck;
int frontCheck;
int rightCheck;
bool left, front, right;
int whiteTotal, prevWhiteLocation, whiteLocation;
//int motorOne, motorTwo;
double whiteRatio;
double prevRatio;
double derivWhite;
double integWhite;
//Maze
signed int leftSensor;
signed int rightSensor;
int whiteWall;
bool noLeftWall, noRightWall, noWallAhead;
int THRESHOLD = 250;
int totalWidth;
signed int leftMotor, rightMotor;
//Primary Initialization
init(1);
//Networking Section
// Send Signal to open gate
connect_to_server("130.195.6.196", 1024);
send_to_server("Please");
receive_from_server(message);
send_to_server(message);
//Line Following Section
set_motor(1, 43);
set_motor(2, -40);
Sleep (5,0);
//Loop runs until both sensors sense walls (start of maze)
while((read_analog(0) < THRESHOLD) || (read_analog(1) < THRESHOLD)){
//Set variables
left = false;
front = false;
right = false;
whiteTotal = 0;
leftCheck = 0;
frontCheck = 0;
rightCheck = 0;
//Take readings
take_picture();
for(i = 0; i < 240; i++){
c = get_pixel(40, i, 3);
if(c > 120){
whiteTotal++;
whiteLocation = whiteLocation + (i-120);
}
}
for(i = 60; i < 70; i++){
c = get_pixel(i, 10, 3);
if(c > 120){
leftCheck++;
}
}
for(i = 60; i < 70; i++){
c = get_pixel(i, 230, 3);
if(c > 120){
rightCheck++;
}
}
for(i = 30; i < 210; i++){
c = get_pixel(160, i, 3);
if(c > 120){
frontCheck++;
}
}
if(leftCheck > 3){
left = true;
}
if(frontCheck > 10){
front = true;
}
if(rightCheck > 5){
right = true;
}
if(left){
set_motor(1, 50);
set_motor(2, 0);
derivWhite = 0.0;
integWhite = 0.0;
Sleep(0, 500000); //Left Sleep
}
else if(front && right){
set_motor(1, 50);
set_motor(2, -50);
derivWhite = 0.0;
integWhite = 0.0;
Sleep(0, 500000); //Front Sleep
}
else if(right){
set_motor(1, 0);
set_motor(2, -50);
derivWhite = 0.0;
integWhite = 0.0;
Sleep(0, 500000); //Right Sleep
}
else if(whiteTotal < 1){
set_motor(1, -50);
set_motor(2, -50);
derivWhite = 0.0;
integWhite = 0.0;
Sleep(0, 300000); //Turn around Sleep
}
else{
derivWhite = ((double)whiteLocation - (double)prevWhiteLocation)/0.01;
integWhite = integWhite + ((double)whiteLocation * 0.01);
whiteLocation = whiteLocation/whiteTotal;
set_motor(1, ((int) ((-(whiteLocation*40/120)*kp+kd*derivWhite)+40)));
set_motor(2, -((int) (((whiteLocation*40/120)*kp+kd*derivWhite)+40)));
// motorOne = (-( ( (whiteLocation*1/3)*kp) + (derivWhite * kd) + (integWhite * ki) + 40))
// motorTwo = (((whiteLocation*1/3)*kp)+(derivWhite * kd) + (integWhite * ki) + 40)
// set_motor(1, motorOne);
// set_motor(2, -motorTwo);
//set_motor(1, ((int)(-( ( (whiteLocation*1/3)*kp) + (derivWhite * kd) + (integWhite * ki) + 40))));
//set_motor(2, -((int) (((whiteLocation*1/3)*kp)+(derivWhite * kd) + (integWhite * ki) + 40)));
prevWhiteLocation = whiteLocation;
Sleep(0,1000);
}
}
while(true){
whiteWall = 0;
//returns true if there isnt a wall
noLeftWall =false;
noRightWall = false;
noWallAhead = false;
//get data from sensors
leftSensor = read_analog(0);
rightSensor = read_analog(1);
//printf("left sensor: %d\nright sensor: %d\n", leftSensor, rightSensor);
//get data from camera
take_picture();
for(int i = 120; i<128; i++){
c = get_pixel(300,i, 3);
if(c>120){ //change white threshold
whiteWall++;
}
}
printf("whiteWall: %d\n", whiteWall);
if(whiteWall < 5){ //Change threshold if theres problems
noWallAhead = true; //rename
// printf("No wall ahead!!!!!!!!!\n\n\n");
}
if(leftSensor<THRESHOLD){
noLeftWall = true;
}
if(rightSensor<THRESHOLD){
noRightWall = true;
}
if(noRightWall){
set_motor(1, 32);
set_motor(2, -30);
Sleep(0, 300000);
set_motor(1, 37);//right motor
set_motor(2, -67);//left motor//CHANGE THRESHOLD
Sleep(0,900000);//CHANGE THRESHOLD
printf("turning right\n");
}
else if(noWallAhead){ //stay in the center of the maze
rightMotor = (leftSensor/10*1.1); //change threshold
leftMotor = -(rightSensor/10);
set_motor(1, rightMotor);
set_motor(2,leftMotor);
Sleep(0,1);
//rotate back to centre
leftMotor = -(leftSensor/10); //change threshold
rightMotor = (rightSensor/10*1.1);
set_motor(1, rightMotor);
set_motor(2, leftMotor);
printf("forwarsdgsdjksdgr\n");
Sleep(0, 1);
}
else if(noLeftWall){
set_motor(1, 40);
set_motor(2,-42);
Sleep(0,450000);
set_motor(1, 66);//right motor
set_motor(2, -32);//left motor //Change thresholds
Sleep(0,900000);
printf("left left left left\n");
}
// else //pop a u turn
/* {
printf("pop a u turn\n");
set_motor(1, -50);
set_motor(2, -60); //bigger so the back doesn't hit the wall
Sleep(0,100000); //Change thresholds
}*/
}
return 0;
}
