/ - Diff - Colony - Robotics Club

     #include <dragonfly_lib.h>
      #ifndef _LINEFOLLOW_H_
      #define _LINEFOLLOW_H_
     #define WHITE			0
     #define GREY			1
     #define BLACK	 		2
     #define CENTER			3
     #define NOLINE			-42
     #define SPEED			200
     #define LINELOST		-1
     /* 	lineFollow_init
     	Must call before lineFollow
     	Turns analog loop off
     */
     void lineFollow_init();
     /*	lineFollow
     	Must call lineFollow first
     	Must be called inside a loop
     */
     int lineFollow();
     /*	assignColor
     	Converts an analog color to WHITE or BLACK
     */
     int assignColor(int port, int analog);
     /*	updateIR
     	Gets the value of the nth line sensor
     */
     int updateIR(int n);
     /*	updateLine
     	Fills the given array with WHITE
     	or BLACK representing the line
     */
     void updateLine(int* values);
     /*	lineLocate
     	Finds the location of the line
     	Outputs positive for right side
     	Negative for left, or NOLINE if a line is not found
     */
     int lineLocate(int* colors);
     //Not implemented yet, returns ???
     int updateBarCode();
     /*	min
     	returns the minimum of two values
     */
     int min(int x, int y);
     /*	motorLeft
     	Commands the left motor
     	Cannot be used to stop
 -126 are backward
 -255 are forward
     */
     void motorLeft(int speed);
     /*	motorRight
     	Commands the right motor
     	Cannot be used to stop
 -126 are backward
 -255 are forward
     */
     void motorRight(int speed);
     /*	lost
     	Internal counter to detect if the line was lost
     */
     int lost;
     #endif

     #include <dragonfly_lib.h>
     #include <lineFollow.h>
     #define WHITE			0
     #define GREY			1
     #define BLACK	 		2
     #define COLOR_BUFFER	10
     #define CENTER			3
     #define NOLINE			-42
     int assignColor(int port, int analog);
     int updateIR(int n);
     void updateLine(int* values);
     int lineLocate(int* colors);
     int straightLineFollow();
     int main(void)
+    {
     	/* initialize components, set wireless channel */
     	dragonfly_init(ALL_ON);
     	straightLineFollow();
     	lineFollow_init();
     	while(1)
     		lineFollow();
     	return 0;
+    }
     //Converts an analog color to the ternary black, grey, white system.
     //If the value is between two codes, the port is read again and the function is called again
     int assignColor(int port, int analog)
+    {
     	int color;
     		if(analog < 341-COLOR_BUFFER)
     			color = WHITE;
     		else if (analog < 341+COLOR_BUFFER)
     			color = assignColor(port, updateIR(port));
     		else if (analog < 682-COLOR_BUFFER)
     			color = GREY;
     		else if (analog < 682+COLOR_BUFFER)
     			color = assignColor(port, updateIR(port));
     		else
     			color = BLACK;
     		return color;
+    }
     //Gets the value of the nth IR sensor
     int updateIR(int n)
+    {
     	//maps the sensors to the analog input ports
     	int ports[5] = {13, 12, 3, 2, 9};
     	return analog10(ports[n]);
+    }
     //Gets the value of the nth IR sensor
     int updateBarCode()
+    {
     	//maps the sensors to the analog input ports
     	int ports[2] = {8,1};
     	return analog10(ports[1]) | analog10(ports[2]);
+    }
     // updates all of the values, and assigns them new colors
     void updateLine(int* values)
+    {
     	int i;
     	for(i = 0; i<5; i++)
     		values[i] = assignColor(i, updateIR(i));
+    }
     // given an array of ints this function will take the arrays weighted average. High numbers indicate that a left turn is needed. Low numberes indicate that a right turn is needed.
     int lineLocate(int* colors)
+    {
     	int i;
     	int wsum = 0;
     	int count=0;
     	for(i = 0; i<5; i++)
+    	{
     		count+=(colors[i]+1)/2;
     		wsum += (i)*colors[i];
+    	}
     	if(count==0)
     			return NOLINE;
     	return (wsum/count)-4;
+    }
     //The robot will follow a line until it is interrupted by a barcode.
     int straightLineFollow()
+    {
     	int colors[6];
     	int location = 0;
     	while(1)
+    	{
     		updateLine(colors);
     		location = lineLocate(colors);
     		//not on line
     		if(location == NOLINE)
+    		{
     			orb1_set_color(GREEN);
     			orb2_set_color(GREEN);
     			motors_off();
+    		}
     		//turn left
     		else if(location > 0)
+    		{
     			orb1_set_color(GREEN);
     			orb2_set_color(ORB_OFF);
     			motor_l_set(FORWARD, 150);
     			motor_r_set(FORWARD, 200);
+    		}
     		//turn right
     		else if(location < 0)
+    		{
     			orb1_set_color(ORB_OFF);
     			orb2_set_color(GREEN);
     			motor_l_set(FORWARD, 200);
     			motor_r_set(FORWARD, 150);
+    		}
     		else
+    		{
     			orb1_set_color(ORB_OFF);
     			orb2_set_color(ORB_OFF);
     			motor_l_set(FORWARD, 200);
     			motor_r_set(FORWARD, 200);
+    		}
+    	}
     	return 0;
+    }

     #include "lineFollow.h"
     void lineFollow_init()
+    {
     	analog_init(0);
     	lost = 0;
+    }
     int lineFollow()
+    {
     	int colors[6];
     	int position;
     	updateLine(colors);
     	position = lineLocate(colors);
     	//not on line
     	if(position == NOLINE)
+    	{
     		if(lost++>20)
+    		{
     			orb1_set_color(GREEN);
     			orb2_set_color(GREEN);
     			motors_off();
     			return LINELOST;
+    		}
+    	}
     	//on line
     	else
+    	{
     		position*=30;
     		orb1_set_color(GREEN);
     		orb2_set_color(ORB_OFF);
     		motorLeft(min(SPEED+position, 255));
     		motorRight(min(SPEED-position, 255));
     		lost=0;
+    	}
     	return 0;
+    }
     void updateLine(int* values)
+    {
     	for(int i = 0; i<5; i++)
     		values[i] = assignColor(i, updateIR(i));
+    }
     int updateIR(int n)
+    {
     	//maps the sensors to the analog input ports
     	int ports[5] = {13, 12, 3, 2, 9};
     	return analog_get10(ports[n]);
+    }
     int assignColor(int port, int analog)
+    {
     	if (analog < 150)
     		return  WHITE;
     	else
     		return  BLACK;
+    }
     int lineLocate(int* colors)
+    {
     	int i;
     	int wsum = 0;
     	int count=0;
     	for(i = 0; i<5; i++)
+    	{
     		count+=(colors[i]+1)/2;
     		wsum += (i)*colors[i];
+    	}
     	if(count==0)
     			return NOLINE;
     	return (wsum/count)-4;
+    }
     int updateBarCode()
+    {
     	//maps the sensors to the analog input ports
     	int ports[2] = {8,1};
     	return analog_get10(ports[1]) | analog10(ports[2]);
+    }
     int min(int x, int y){return x>y ? y : x;}
     void motorLeft(int speed){
     	speed-=127;
     	if(speed>=0)motor_l_set(FORWARD, 160+speed*95/128);
     	else motor_l_set(BACKWARD, 160-speed*95/127);
+    }
     void motorRight(int speed){
             speed-=127;
             if(speed>=0)motor_r_set(FORWARD, 160+speed*95/128);
             else motor_r_set(BACKWARD, 160-speed*95/127);
+    }

     USE_WIRELESS = 1
     # com1 = serial port. Use lpt1 to connect to parallel port.
     AVRDUDE_PORT = $(shell if uname -s |grep -i w32 >/dev/null; then echo 'COM4:'; else echo '/dev/tty.usbserial-*'; fi)
     AVRDUDE_PORT = $(shell if uname -s |grep -i w32 >/dev/null; then echo 'COM4:'; else echo '/dev/ttyUSB0'; fi)
     else
     COLONYROOT := ../$(COLONYROOT)

Project

General

Profile

Colony

Revision 1841