/ - Diff - Colony - Robotics Club

     #define MIN_IR_ADC8 20
     #define MAX_IR_ADC8 133
     // hacky conversion
     #define IR_MAX 800
     #define IR_MIN 100
     #define IR_CONVERSION_FACTOR 2.0
     void range_init(void)
+    {
     	/* simulator does not need this */
+    }
     // should return value between 101 and 800
     int range_read_distance (int range_id)
+    {
     	int temp=0;
     	short temp;
     	short ir_value; // simulate what libdragonfly rangefiner returns
     	/* read from array in shared memory, if valid */
     	if (range_id > 0 && range_id < 6) {
     		temp = (shared_state->ranges).d[range_id];
     		if (temp < 0 || temp > 1000)
     			return -1;
     		else
     			return temp;
     	/* TODO: fix numbering in libdragonfly then fix it here */
     	switch (range_id) {
     	case IR1:
     		temp = (shared_state->ranges).d[0];
     		break;
     	case IR2:
     		temp = (shared_state->ranges).d[1];
     		break;
     	case IR3:
     		temp = (shared_state->ranges).d[2];
     		break;
     	case IR4:
     		temp = (shared_state->ranges).d[3];
     		break;
     	case IR5:
     		temp = (shared_state->ranges).d[4];
     		break;
+    	}
     	ir_value = ((temp / IR_CONVERSION_FACTOR) + IR_MIN);
     	ir_value = ir_value > IR_MAX ? IR_MAX : ir_value;
     	printf("[%d]: %d\n", range_id, ir_value);
     	if (temp >= 0 && temp <= 1000) return ir_value;
     	return -1;
+    }

     #define TIME 1 /*sec*/
     #define ROBOT_WIDTH 131 /*mm*/
     #define MOTOR_CONVERSION_FACTOR 1000.0
     #define MOTOR_CONVERSION_FACTOR 10.0
     #define FUDGE 10 /* minimum rangefinder distance until collision */
-...
      **/
     int move_robot(Robot* r)
+    {
         printf("Called motion\n");
     	Pose old_pose = r->pose;
     	short speed1 = r->shared->motor1;
-...
     	  r->pose.y += sin(theta) * speed1 / MOTOR_CONVERSION_FACTOR;
     	  for (divide = 0; divide < 5; divide++) {
     	    /* Lets just call this a collision... */
     	    printf("%d: %d\n",divide,r->shared->ranges.d[divide]);
     	    //printf("%d: %d\n",divide,r->shared->ranges.d[divide]);
     	    if (r->shared->ranges.d[divide] < FUDGE) {
     		/* Restore x,y, but allow rotation */
     		r->pose.x = old_pose.x;
-...
     		update_rangefinders(r);
     		return 0;
+    	    }
+    	}
+    		}
     	  return 0;
+    	}
     	radius = ROBOT_WIDTH * speed1 / (speed1 - speed2);
-...
     	update_rangefinders(r);
     	for (divide = 0; divide < 5; divide++) {
     	    /* Lets just call this a collision... */
     	    printf("%d: %d\n",divide,r->shared->ranges.d[divide]);
     	    //printf("%d: %d\n",divide,r->shared->ranges.d[divide]);
     	    if (r->shared->ranges.d[divide] < FUDGE) {
     		/* Restore x,y, but allow rotation */
     		r->pose.x = old_pose.x;

     #include "world.h"
     //TODO: These need to be measured...
     double rf_thetas[5] = {0.0, 2*M_PI/5, 4*M_PI/5, 6*M_PI/5, 8*M_PI/5};
     double rf_thetas[5] = {8*M_PI/5, 0.0, 2*M_PI/5, 4*M_PI/5, 6*M_PI/5};
     //double rf_thetas[5] = {0.0, 0.0, M_PI/2, M_PI, 3*M_PI/2};
     void update_rangefinders(Robot *bot)
-...
+        {
     	rf.d = theta + rf_thetas[ir];
     	x = collide_world(&rf);
     	//printf("@(%g) - [%d] = %g --> %d\n",rf.d, ir, x, (short)x);
     	// printf("@(%g) - [%d] = %g --> %d\n",rf.d, ir, x, (short)x);
     	bot->shared->ranges.d[ir] = x;
+        }
+    }

branches/simulator/projects/simulator/test/world.txt
4	4	POLYGON 4 CONNECTED -999 1000 -999 999 999 999 999 1000
5	5	POLYGON 4 CONNECTED 1000 -1000 1000 1000 999 1000 999 -1000
6	6
7		POLYGON 4 CONNECTED -20 -20 -20 20 20 20 20 -20
	7	#POLYGON 4 CONNECTED -20 -20 -20 20 20 20 20 -20
	8	POLYGON 4 CONNECTED -100 -500 -100 500 -80 500 -80 -500
8	9

     #include <dragonfly_lib.h>
     #include "smart_run_around_fsm.h"
     int main(void)
+    {
     	dragonfly_init(ALL_ON);
     	bom_leds_on(BOM_ALL);
     	motor1_set(1, 200);
     	motor2_set(1, 150);
     	//bom_leds_on(BOM_ALL);
     	//motor1_set(1, 210);
     	//motor2_set(1, 200);
     	run_around_init();
     	while(1){
            simulator_do();
+      }
     		run_around_FSM();
            		simulator_do();
+      	}
     	return 0;
+    }

     #include "dragonfly_lib.h"
     #include "smart_run_around_fsm.h"
     /*A simple behavior for navigating in an environment, i.e. avoiding walls and getting stuck.
     Could be better at not getting stuck.
     Latest revision only has two accessible states: move and reverse.
     */
     void run_around_init(void)
+    {
       range_init();
       /*Start in the default state, MOVING*/
       avoid_state=MOVING;
       /*Set timers to their maximum values.*/
       crazy_count=CRAZY_MAX;
       backup_count=BACKUP_MAX;
       pControl=0;
       /*Initialize distances to zero.*/
       d1=0; d2=0; d3=0; d4=0; d5=0;
+    }
     /*The main function, call this to update states as frequently as possible.*/
     void run_around_FSM(void) {
       /*Default to moving.*/
       avoid_state=MOVING;
       /*The following lines ensure that undefined (-1) values
       will not update the distances.*/
       int temp;
       temp=range_read_distance(IR1);
       d1=(temp == -1) ? d1 : temp;
       temp=range_read_distance(IR2);
       d2=(temp == -1) ? d2 : temp;
       temp=range_read_distance(IR3);
       d3=(temp == -1) ? d3 : temp;
       temp=range_read_distance(IR4);
       d4=(temp == -1) ? d4 : temp;
       temp=range_read_distance(IR5);
       d5=(temp == -1) ? d5 : temp;
       /*If the crazy count is in it's >>3 range, it acts crazy.*/
       if(crazy_count<=(CRAZY_MAX>>3))
+      {
         avoid_state=CRAZY;
         crazy_count--;
         if(crazy_count<0) crazy_count=CRAZY_MAX;
         evaluate_state();
         return;
+      }
       //Checks the forward distance to see if it should back up, if so...state backwards.
       printf("d2: %d\n",d2);
       if((d2!=-1)&&(d2 < 150)){
           backup_count=BACKUP_MAX;
           avoid_state=BACKWARDS;
           evaluate_state();
           return;
+      }
       /*
       if(d1 < 120 || d3 < 120) {
     		avoid_state = BACKWARDS;
     		backup_count = BACKUP_MAX;
     		evaluate_state();
     		return;
+      }
       */
       if(backup_count<BACKUP_MAX){
         avoid_state=BACKWARDS;
         if(backup_count<0)
           backup_count=BACKUP_MAX;
         evaluate_state();
         return;
+      }
       /*Should evaluate an expression from -255 to 255 to pass to move.*/
       pControl= ((d3-d1) + (d4-d5)) >> TURN_CONSTANT;
       if(pControl>PCONTROL_CRAZY_LIMIT || pControl<-PCONTROL_CRAZY_LIMIT) crazy_count--;
       /*i.e. if you really want to turn for an extended period of time...you're probably stuck.*/
       /*Debug stuff:*/
       /*usb_puts("pControl evaluating: ");
       usb_puti(pControl);
       usb_puts("\n\r");
       usb_puts("IR1: ");
       usb_puti(d1);
       usb_puts(" IR2: ");
       usb_puti(d2);
       usb_puts(" IR3: ");
       usb_puti(d3);
       usb_puts(" IR4: ");
       usb_puti(d4);
       usb_puts(" IR5: ");
       usb_puti(d5);
       usb_puts("\n\r");*/
       evaluate_state();
+    }
     //Acts on state change.
     void evaluate_state(){
         switch(avoid_state){
         case(MOVING):
           move(STRAIT_SPEED,-pControl);
           break;
         case(BACKWARDS):
           move(-STRAIT_SPEED-50,0);
           break;
         case(CRAZY):
           /*TODO: Implement a crazy state.*/
           move(STRAIT_SPEED,-pControl);
           break;
         default:
           /*Should never get here, go strait.*/
           move(100,0);
           break;
+      }
+    }

     //Obstacle Avoid Numbers
     #ifndef _RUN_AROUND_FSM_H_
     #define _RUN_AROUND_FSM_H_
     //The States:
     #define MOVING 12           //Move strait.
     #define BACKWARDS 15        //Move backwards. (Front close to wall.)
     #define STOP 16             //Stop.  The default state, (Something broke).
     #define CRAZY 40            //Erratic behavior that occurs more often when the robot is frequently trying to turn. (i.e. may be stuck.)
     #define LEFT 37             //Left
     #define RIGHT 39            //Right
     #define BACKUP_MAX 60
     #define CRAZY_MAX 200       //The number of counts between "crazy moments"
     #define STRAIT_SPEED 185    //The speed when going strait or backing up.
     #define TURN_CONSTANT 2
     #define PCONTROL_CRAZY_LIMIT 80
     int avoid_state;
     int prev_state;    /*State machine variable.*/
     int crazy_count;    /*Counter for a 'get unstuck' behavior.*/
     int backup_count;	/*Counter for backup duration.*/
     int pControl;		/*Proportional control variable, determines turn direction.*/
     int d1,d2,d3,d4,d5;	/*The five distances taken in by IR.*/
     void run_around_init(void);
     void run_around_FSM(void);
     void evaluate_state(void);
     #endif

Project

General

Profile

Colony

Revision 1094