/ - Diff - Colony - Robotics Club

Revision 1342

Added by Andrew Yeager almost 15 years ago

sort of works, does the 45 line correctly.

branches/encoders/code/behaviors/spline/server/speed_map.h
	1	#ifndef SPEED_MAP_H
	2	#define SPEED_MAP_H
	3
	4	double real_velocity(int, int);
	5	int robot_velocity(int, double);
	6
	7	#endif

     #define RS 2
     #define RD 3
     #define LEFT 0
     #define RIGHT 1
     #define FORWARD 1
     #define BACKWARD 0
     #define SENDER 1
     #define RECEIVER 0
     #define INFINITE -1
     #define L 10.0
     #define L 13.5
     #define DT .1
     #define DT .102
     #define DIST(x1, y1, x2, y2)    (sqrt(pow(((x1)-(x2)), 2) + pow(((y1)-(y2)), 2)))
     #define LOOK_AHEAD              1
     #define LOOK_AHEAD_ERR_THRES    0.05
     #define VELOCITY                1
     #define DIST(x1, y1, x2, y2)    (sqrt(((x1)-(x2))*((x1)-(x2)) + ((y1)-(y2))*((y1)-(y2))))
     #define LOOK_AHEAD              30
     #define LOOK_AHEAD_ERR_THRES    5
     #define VELOCITY                20
     typedef struct state
+    {
-...
     	double y;
     	double theta;
     	short vl;
     	short vr;
     	double vl;
     	double vr;
     	int vl_ref;
     	int vr_ref;
     	double vl_ref;
     	double vr_ref;
     } state;
     typedef struct {
-...
     void setAttriv(int file);
     void pos_control_curv(curv_t *ref, int len);
     void pos_control_curve(curv_t *ref, int len);
     void velocityControl(double*, double*);

     #include "encoders.h"
     #include "internals.h"
     #include <math.h>
     #include "speed_map.h"
     #include <time.h>
     #define PG 150
     #define DG 100
     #define IG 10
     #define PG 8
     #define DG .1
     #define IG .1
     int VEL_S = 1;
     #define TICKS_PER_CM 48.03
     #define TIME_INTERVAL 1520
     #define LEN 30000
     state robot;
     double vel_l_i;
     double vel_r_i;
-...
     	robot.theta = theta;
     	robot.vl = 0;
     	robot.vr = 0;
     	robot.vr = 0;
     	robot.vl_ref = 0;
     	robot.vr_ref = 0;
+    }
-...
     	attributes.c_cflag &= ~PARENB;
     	attributes.c_cflag &= ~CSTOPB;
     	attributes.c_cflag &= ~CSIZE;
     	attributes.c_cflag |= CS8;
     	attributes.c_cflag |= 	CS8;
     	attributes.c_cflag &= ~ICRNL;
     	attributes.c_cflag &= ~OCRNL;
-...
+    	}
+    }
     int main()
     int main(int argc, char** args)
+    {
         init(50, 0, M_PI/2);
     	//init(0,0,0);
     	double x_ref[LEN];
     	double y_ref[LEN];
         int i;
     	for (i = 0; i < LEN; i++)  {
         	x_ref[i] = 50 * cos((double)(i+1) / LEN * M_PI);
         	y_ref[i] = 50 * sin((double)(i+1) / LEN * M_PI);
        // 	x_ref[i] = (double)i / LEN * 100;
        //` 	y_ref[i] = (double)i / LEN * 100;
     		//printf("x_ref %g \t y_ref %g\n", x_ref[i], y_ref[i]);
     		//getchar();
+    	}
     	curv_t ref;
     	ref.x = x_ref;
     	ref.y = y_ref;
     	int serialFileIn = open("/dev/ttyUSB0", O_RDWR);
     	int serialFileOut = open("/dev/ttyUSB1", O_RDWR);
     	if(serialFileIn < 1 || serialFileOut < 1)
-...
     	setAttrib(serialFileOut);
     	setAttrib(serialFileIn);
     	unsigned char encoders[4] = {1,1,1,1};
     	//unsigned char encoders[4] = {0,0,0,0};
     	char ref_vels[2] = {0,0};
     	char senderNum = SENDER;
     	char receiverNum = RECEIVER;
-...
     	write(serialFileOut, &senderNum, 1);
     	//Sending velocity as LS LD RS RD
     	int tempCount = 190;
     	int tempCount = 0;
     	int dx, left_v;
     	int dx;
     	double Ul, Ur;
     	while(1)
+    	{
     	while(1) {
     		printf("SENDING DATA\n");
             velocityControl(&Ul, &Ur);
     	    if(Ul > 0)
     			encoders[LD] = FORWARD;
     		else
     			encoders[LD] = BACKWARD;
     		encoders[LS] = (unsigned char)abs(Ul);
             pos_control_curve(&ref, LEN);
     //		ref_vels[LEFT] = 20;
     //		ref_vels[RIGHT] = 20;
             if(Ur > 0)
     			encoders[RD] = FORWARD;
     		else
     			encoders[RD] = BACKWARD;
     		encoders[RS] = (unsigned char)abs(Ur);
     		ref_vels[LEFT] = -(char)robot_velocity(1, robot.vl_ref);
     		ref_vels[RIGHT] = -(char)robot_velocity(1, robot.vr_ref);
     //        printf("Ref Vs: \t\t%d\t%d\n", ref_vels[LEFT], ref_vels[RIGHT]);
            // printf("Left Speed: %d \t Right Speed: %d\n", encoders[LS], encoders[RS]);
     		//tempCount++;
     		//printf("RECEIVED %d\n", tempCount);
     		int temp = 0;
     		int count = 0;
     		do
+    		{
     			temp = write(serialFileOut, encoders + count, 1);
     			temp = write(serialFileOut, ref_vels + count, 1);
     			if(temp < 0)
     				perror("Write Error");
     			count += temp;
     			printf("sent: %d\n", count);
     			usleep(200);
     		}while(count < 4);
     			//printf("sent: %d\n", count);
     			usleep(10000);
     		   // sleep(1);
     		} while(count < 2);
     		count = 0;
     		printf("READING DATA\n");
     		//printf("READING DATA\n");
     		do
+    		{
     			count += read(serialFileIn, encoders, 4);
     		}while(count < 4);
     			count += read(serialFileIn, ref_vels, 2);
     		}while(count < 2);
     		robot.vl = (encoders[0] << 8) | encoders[1];
     		robot.vr = (encoders[2] << 8) | encoders[3];
     		//printf("%d %d\n", robot.vl, robot.vr);
     		robot.vl = real_velocity(1, -ref_vels[LEFT]);
     		robot.vr = real_velocity(1, -ref_vels[RIGHT]);
       //  	printf("ACTUAL V \t\t%g\t%g\n", robot.vl, robot.vr);
     		updatePosition();
     		printf("Location is x: %g \t y: %g \t o: %g\n", robot.x,robot.y, robot.theta);
     		//printf("Raw Location is x: %g \t y: %g \t o: %g\n", robot.x,robot.y, robot.theta);
               printf("%g\t%g\n", robot.x, robot.y);
+    	}
     	return 0;
+    }
     void scaleInput(double* input)
+    {
     	printf("INPUT: %g\n", *input);
       	if(*input > 0)
+    	{
     		(*input) += 170;
     		//(*input) += 170;
     		if(*input > 210)
     			*input = 210;
+    	}
     	else
     	else if((*input) < 0)
+    	{
     		(*input) -= 170;
     		//(*input) -= 170;
     		if(*input < -210)
     			*input = -210;
+    	}
+    }
     void velocityControl(double* Ul, double* Ur)
+    {
     	double vel_l_e = robot.vl_ref - robot.vl;
     	double vel_r_e = robot.vr_ref - robot.vr;
     	vel_l_i = vel_l_i + vel_l_e;
     	vel_r_i = vel_r_i + vel_r_e;
     	(*Ul) = PG * vel_l_e + DG * (vel_l_e - vel_l_old) + IG * vel_l_i;
     	(*Ur) = PG * vel_r_e + DG * (vel_r_e - vel_r_old) + IG * vel_r_i;
     	scaleInput(Ul);
     	scaleInput(Ur);
     	vel_l_old = vel_l_e;
     	vel_l_old = vel_r_e;
+    }
     //Todo: This makes no sense, 4am
     void updatePosition()
+    {
        	double wdt = (robot.vr - robot.vl)*DT/L;
        	double R;
        	if(robot.vr - robot.vl > 0)
        	if(robot.vr - robot.vl != 0)
        		R = (L/2)*(robot.vr + robot.vl)/(robot.vr - robot.vl);
     	if(wdt != 0)
+    	{
     		double ICC[2] = {robot.x - R * sin(robot.theta), robot.y + R*cos(robot.theta)};
     		robot.x = cos(wdt)*(robot.x-ICC[0]) - sin(wdt)*(robot.y-ICC[1]) + ICC[0];
     		robot.y = sin(wdt)*(robot.x-ICC[0]) + cos(wdt)*(robot.y-ICC[1]) + ICC[1];
     		robot.x = (cos(wdt)*(robot.x-ICC[0]) - sin(wdt)*(robot.y-ICC[1]) + ICC[0]);
     		robot.y = (sin(wdt)*(robot.x-ICC[0]) + cos(wdt)*(robot.y-ICC[1]) + ICC[1]);
     		robot.theta = robot.theta + wdt;
+    	}
     	else
+    	{
     	   	robot.x += cos(robot.theta)*robot.vr;
     		robot.y += sin(robot.theta)*robot.vr;
     	   	robot.x += cos(robot.theta)*robot.vr*DT;
     		robot.y += sin(robot.theta)*robot.vr*DT;
+    	}
     	if (robot.theta > 2*M_PI)
     		robot.theta -= 2*M_PI;
     	if (robot.theta < -2*M_PI)
     		robot.theta += 2*M_PI;
+    }
     static int find_goal(curv_t *ref, int len);
     void pos_control_curv(curv_t *ref, int len) {
     void pos_control_curve(curv_t *ref, int len) {
         int goal_idx = find_goal(ref, len);
         //did a transformation here.
         double goal_y = cos(robot.theta)*(ref->y[goal_idx]-robot.y)
             - sin(robot.theta)*(ref->x[goal_idx]-robot.x);
         double goal_rad = 2*goal_y / LOOK_AHEAD;
         robot.vl_ref = VELOCITY*(1-0.5*goal_rad);
         robot.vr_ref = VELOCITY*(1+0.5*goal_rad);
         //printf("GOAL: y: %g,  o: %g\n", goal_y, goal_rad);
         robot.vl_ref = real_velocity(1, VELOCITY)*(1-0.5*goal_rad);
         robot.vr_ref = real_velocity(1, VELOCITY)*(1+0.5*goal_rad);
         /*
     	if(robot.vl_ref > 127)
     		robot.vl_ref = 127;
     	else if(robot.vl_ref < -128)
     		robot.vl_ref = -128;
     	if(robot.vr_ref > 127)
     		robot.vr_ref = 127;
     	else if(robot.vr_ref < -128)
     		robot.vr_ref = -128;
     		*/
+    }
     static int find_goal(curv_t *ref, int len) {
-...
                 min_idx = i;
+            }
+        }
     	//printf("%d min %f\n", min_idx, min_dist);
     	//getchar();
         goal_idx = -1;
         for (i = min_idx; i < len; i++) {
             temp = DIST(ref->x[i], ref->y[i], robot.x, robot.y);
             if (abs(temp - LOOK_AHEAD) < LOOK_AHEAD_ERR_THRES) {
                 goal_idx = i;
                 break;
+            }
             	return goal_idx;
+    		}
+        }
     	while(1) {
     		printf("eol\n");
     	    exit(0);
+    	}
         return goal_idx;
+    }
+    }

branches/encoders/code/behaviors/spline/server/Makefile
1	1	all: server.c
2		gcc -lm encoders.c server.c -o server
	2	gcc -lm -Wall speed_map.c server.c -o server
3	3
4	4	clean:
5	5	rm server

     #include "speed_map.h"
     double robot_1[41] =
+    {
 ,
 .495185695,
 .935064935,
 .385681293,
 .834862385,
 .278481013,
 .739726027,
 .20855615,
 .629032258,
 .295942721,
 .7,
 .013927577,
 .487804878,
 .940594059,
 .382978723,
 .844106464,
 .317073171,
 .75862069,
 .181818182,
 .653846154,
 .183673469,
 .677419355,
 .05586592,
 .34482759,
 .97560976,
 .39240506,
 .84210526,
 .4137931,
 .76595745,
 .23529412,
 .63636364,
 .17322835,
 .51612903,
 ,
 .65217391,
 .78947368,
 .51376147,
 .66666667,
 .47572816,
 .84,
 .3
     };
     int robot_velocity(int robot, double velocity)
+    {
     	if(velocity > 0)
     		return (int)((velocity+0.2757)/0.4645);
     	velocity = -velocity;
     	return -(int)((velocity+0.2757)/0.4645);
+    }
     double real_velocity(int robot, int velocity)
+    {
     	if(velocity > 40 || velocity < -40)
     		return velocity < 0?-18.3:18.3;
     	switch(robot)
+    	{
     		case 1:
     			return velocity < 0?-robot_1[-velocity]:robot_1[velocity];
     		default:
     			exit(-15410);
+    	}
+    }

Also available in: Unified diff

Project

General

Profile

Colony

Revision 1342