View unanswered posts | View active topics It is currently Sat Nov 18, 2017 2:34 am






Reply to topic  [ 1 post ] 
EV3 as a measurment tool 
Author Message
Rookie

Joined: Sat Apr 20, 2013 1:02 pm
Posts: 16
Location: Israel
Post EV3 as a measurment tool
Hi,

I want to share my project below that helped me to get into the wonderful world of robotC with EV3 hardware.
I am adding the source code below. Feel free to use it for noncommercial usage.
Mail me for any question.

operation:
https://www.youtube.com/watch?v=AAuMDGnGMjU

EV3 being used here as a tool to measure a table size with one color sensor and a gyro.
Measure results are sent into the PC debugger.
Detailed explanation is inside the code header.

Enjoy,
Amit

***********************************************************************
RobotC source Code:
***********************************************************************

#pragma config(Sensor, S1, gyroSensor, sensorEV3_Gyro)
#pragma config(Sensor, S4, Color, sensorEV3_Color)
#pragma config(Sensor, S3, US, sensorEV3_Ultrasonic)
#pragma config(Motor, motorB, leftMotor, tmotorEV3_Large, PIDControl, driveLeft, encoder)
#pragma config(Motor, motorC, rightMotor, tmotorEV3_Large, PIDControl, driveRight, encoder)
#pragma config(DatalogSeries, 0, "Color", Sensors, Sensor, S3, 50)
#pragma config(DatalogSeries, 1, "MotorB", Motors, MotorEncoder, motorB, 50)
#pragma config(DatalogSeries, 2, "MotorC", Motors, MotorEncoder, motorC, 50)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//





/*
Date : 12012016
Project Target:
---------------
Target of this program is to find the dimensions (X*Y) of a rectangular flat table without edges (dining table)
Robot will detect the table edges and not falling down using the color sensor pointing down.
A short video of the edge detection mechanism is shown at: https://www.youtube.com/watch?v=uDj8odiibdE
This just demonstrate the functionality of edge detection using the color sensor.

You can use this program with the permition from the autor: amit.bronstein@gmail.com

HW used:
--------
1. Education LEgo set.
2. Lego Gyro sensor
3. Lego Color sensor
4. Simple Bot with 2 big motors and rear metal ball (see construction in the movie)

General:
--------
In this program (Movie at youTube: https://www.youtube.com/watch?v=AAuMDGnGMjU ), Starting point: the robot can be placed anywhere around a table pointing at any direction.
The Robot will travel till the edge of the table is detected, and will rotate until it is alined exactly 90deg with the edge, and then turn 180deeg and go to the oposite direction,
while measuring the distance. Same will be done for both table edges, and at the end a comment will be send to the computer debug browser with he exact dimentions found.
The accuracy of the measure is around 1.5%.
THe turning is done using the Gyro measurments and the rotation is based on PID mechansim.

Details:
--------
- At the begining there are few positions that are not allowed becauae of failing risk. In each of those the robot will stop and will ask to be re-placed.
This is done in order to minimize the risk of falling from table.
There are stil rare cases that falling may happpend. Make sure to keep an eye on the robot operation at any time.

Situations are as follows:
1. Sesnor is placed out of the table. robot will not move (5 beeps will be heared)
2. While reaching the edge, the robot understand that it is exactly located on table corner (while scanning as explained below).
This is a bit risky and thereofore we wil stop (5 beeps will be heared).
3. At the begining of operation robot will do a 360deg scan. If an "out of table" was found it will stop. It means that the location is also risky for falling (5 beeps will be heared).

If all the conditions above are met, we start the measuring process. The stages are as below:

1. From staring point, robot will move is a straight line toward edge of table. Color sensor that is pointing down will detect when it is out of table
and the robot will fully stop.
2. After a complete stop the robot will move about 5cm ahead in order to make sure that the sensor is out of the table so it can be used as a "Protractor" to measure the external angle.
3. Robot will now rotate left until the sensor hits the table edge again and will stop.
4. This is a starting position for measuring the external angle.
5. Robot will slowly move the color sensor to the other direciton until meeting the table edge again, and stop
6. Once stop, we know the exact angle (<180 deg), and we now tell the robot to turn 1/2 of that angle back.
This is done using an accurate PID controller. At this point robot is 90 deg allined to the table boundary.
7. Robot will now travel back until the sensor is on edge again, and now rotate exactly 180 deg back (there is a complensation meachansim here since
the backward movement is not accurtate as the robot is designed to go only straight).
8. The exact length of the robot is known (form the point that the sensor is on edge to the oposite position of the sensor after turning 180 deg).
9. Now it travels to other side and the distance is being measured using the robot encoder.
10. One dimentions is known by now and the robot turn 90deg and start the same process of measuring the other side.

*/




/********************************************/
/* PID definitions */
/********************************************/
#define Kp 0.05292 // Kp
#define Ki 0.000000075 // Ki
#define Kd 0.0375 //Kd


// We have 8 possible places to store data, we call each one a datalog series
// This example uses the first four to store values.
#define DATALOG_SERIES_0 0
#define DATALOG_SERIES_1 1
#define DATALOG_SERIES_2 2
#define DATALOG_SERIES_3 3
#define DATALOG_SERIES_4 4
#define DATALOG_SERIES_5 5


/////////////////////////////////
// START define functions
/////////////////////////////////
// produce K number of beeps form 400Hz increasing by 200Hz each time.
void multi_beep (int k)
{
int i;

for (i = 0 ; i < k ; i = i + 1 ) {
playTone(400+i*200, 20);
sleep(20);
}
}
/********************************************************************/
/* Usage: */
/* For clockwise turn exactTurn(deg-of-turn , 30 ,1000, 1) */
/* For counter-clockwise turn exactTurn (deg-of-turn , 30 ,1000, -1) */
/* logme=1 will activate the log file */
/********************************************************************/
void exactTurn(long deg, int speed, int stable_time, int right_1_left_m1, int logme)
{
long initial_deg, desired_deg ;
initial_deg = getGyroDegrees(gyroSensor);
desired_deg = initial_deg + deg;

// this flag is set for type first time that the "PID fix target" has been reached.
// Once it is set it is not possible any more to go to maximum speed
int fix_pid_limit = 0;
int exit_flag1 = 0;
int time_point1 = 0;
int stability_achieved = 0;
float k_speed = 1;
// initialize all variables for both motor B and motor C
int actualDegrees=getGyroDegrees(gyroSensor);


int error=0;
int pre_err=0;
int I=0;

int D=0;
float output;

time1[T1]=0;
while(time1[T1]<stable_time)
{
// Reading current Gyro (assuming it is zero)
actualDegrees=right_1_left_m1*getGyroDegrees(gyroSensor); // Reading current deg from gyro
error=desired_deg-actualDegrees; // find error of motor B

I=I+error; // find value of integral
D=error-pre_err; // find value of derivative
output=Kp*error+Ki*I+Kd*D; // pid contorl for rotation
pre_err=error; // store the error

// The motors will run full speed until getting close to the target
if (error > 10 & fix_pid_limit == 0) {
motor[motorB]=speed*right_1_left_m1;
motor[motorC]=-speed*right_1_left_m1;
} else {
fix_pid_limit = 1;
motor[motorB]=speed*right_1_left_m1*k_speed*output;
motor[motorC]=-speed*right_1_left_m1*k_speed*output;

}

// datalogAddValue when surrounded by datalogDataGroupStart and datalogDataGroupEnd
// adds several values to one row of the datalog with a common time stamp
if (logme == 1) {
datalogDataGroupStart();
datalogAddValue( DATALOG_SERIES_0, actualDegrees );
datalogAddValue( DATALOG_SERIES_1, error );
datalogAddValue( DATALOG_SERIES_2, output );
datalogAddValue( DATALOG_SERIES_3, fix_pid_limit );
datalogDataGroupEnd();
}

sleep(1);
if (error==0 & exit_flag1 == 0) { time_point1 = time1[T1]; exit_flag1 = 1;}
if (error==0 & exit_flag1 == 1 & time1[T1] > time_point1 + 500) {
stability_achieved = 1;
multi_beep (2);
sleep(500);
break;
;
}

}
}



void waitAndBeep( int i )
{
sleep (i);
playTone(400, 20);
while(bSoundActive) { sleep(1);}
}

void waitForStopAndBeep()
{
waitUntilMotorStop(leftMotor);
waitUntilMotorStop(rightMotor);
playTone(200, 20);
while(bSoundActive) { sleep(1);}
}



void syncMove (float how_many_180deg_distances , float deg180_motor_turn, int power, int motor_relation)
{
// reset both motors
resetMotorEncoder(leftMotor);
resetMotorEncoder(rightMotor);

setMotorSyncEncoder(leftMotor, rightMotor, motor_relation, how_many_180deg_distances * deg180_motor_turn , power);
waitForStopAndBeep();
}


// This function will scan a full circle and mark the value for each sensor deg.
// It should return an array that contain ther sensor value for each deg
int scan_clockwise (int power, int threshold)
{
resetGyro(gyroSensor);
sleep(500);
resetGyro(gyroSensor);
sleep(200);

datalogDataGroupStart();
datalogAddValue( DATALOG_SERIES_1, getGyroDegrees(gyroSensor) );
datalogAddValue( DATALOG_SERIES_4, 998 );
datalogDataGroupEnd();
setMotorSync(leftMotor, rightMotor, 100, power);

// since this proc is activated when the sensor is over table we first need to wait for first exit from table
while ( SensorValue[Color] > threshold) {
sleep(1);
}
sleep(100);
while (SensorValue[Color] < threshold) {
// actualDegrees = getGyroDegrees(gyroSensor);
// currentDistance_color = SensorValue[Color];

sleep(10);

// datalogDataGroupStart();
// datalogAddValue( DATALOG_SERIES_0, actualDegrees );
// datalogAddValue( DATALOG_SERIES_1, currentDistance_color );
// datalogDataGroupEnd();
}

motor[leftMotor] = 0;
motor[rightMotor] = 0;
waitForStopAndBeep();
sleep(500);
datalogDataGroupStart();
datalogAddValue( DATALOG_SERIES_1, getGyroDegrees(gyroSensor) );
datalogAddValue( DATALOG_SERIES_4, 997 );
datalogDataGroupEnd();
sleep(500);
return getGyroDegrees(gyroSensor);
}


/***************
scan_360
***************/
int scan_360 (int power, int threshold)
{

int out_of_table_error = 0;
resetGyro(gyroSensor);
sleep(500);
resetGyro(gyroSensor);
sleep(200);

setMotorSync(leftMotor, rightMotor, 100, power);

// since this proc is activated when the sensor is over table we first need to wait for first exit from table
while ( getGyroDegrees(gyroSensor) < 360 ) {
sleep(10);
if (SensorValue[Color] < threshold) {out_of_table_error = 1;}
if (0) {
datalogDataGroupStart();
datalogAddValue( DATALOG_SERIES_1, getGyroDegrees(gyroSensor) );
datalogAddValue( DATALOG_SERIES_2, out_of_table_error );
datalogAddValue( DATALOG_SERIES_3, SensorValue[Color] );
datalogAddValue( DATALOG_SERIES_4, 990 );
datalogDataGroupEnd();
}
}
sleep(100);

motor[leftMotor] = 0;
motor[rightMotor] = 0;
waitForStopAndBeep();
sleep(500);
return out_of_table_error;
}



// This will move the sensor head to left until meeting table (sensoe is out of table)
int move_until_enter_edge_from_outside (int power, int threshold, int motor_ratio)
{
// Note that the encoder here counts only forward. It is currenlty not supporting backward movement of the robot.

setMotorSync(leftMotor, rightMotor, motor_ratio, power);
while (SensorValue[Color] < threshold) {
sleep(1);
}
// stop after 360deg (full circle)
motor[leftMotor] = 0;
motor[rightMotor] = 0;
waitForStopAndBeep();
sleep(500);
return 1;
}


int move_until_enter_edge_from_inside (int power, int threshold, int motor_ratio)
{
// Note that the encoder here counts only forward. It is currenlty not supporting backward movement of the robot.

setMotorSync(leftMotor, rightMotor, motor_ratio, power);
while (SensorValue[Color] > threshold) {
sleep(1);
}
// stop after 360deg (full circle)
motor[leftMotor] = 0;
motor[rightMotor] = 0;
waitForStopAndBeep();
sleep(500);
return 1;
}

int motor_enc_value ()
{
int enc_motor_B, enc_motor_C, enc_value;
enc_motor_B = nMotorEncoder[motorB];
enc_motor_C = nMotorEncoder[motorC];
enc_value = (enc_motor_B + enc_motor_C) /2 ;
return enc_value;
}

//////////////////////////////////////////////////
/////////////////// MAIN /////////////////////////
//////////////////////////////////////////////////




//TEV3LEDPatterns led_color;

task main()
{
float Kfactor , wheel_diameter , robot_width , wheel_width , deg180_motor_turn;
int currentDistance;
int start_flag , move_flag , emergency_stop;
int threshold , threshold_hand;
int deg, rotate_back;
long turn_degree;

//////////////////////////
// Initial values
//////////////////////////
wheel_diameter = 56;
wheel_width = 28;
robot_width = 145;
Kfactor = 1.02;
deg180_motor_turn = Kfactor *360 *(robot_width - wheel_width) / wheel_diameter /2 ;
threshold = 3;
threshold_hand = 6;
start_flag = 1;
move_flag = 1; // disbled the emergency stop
emergency_stop = 0;
int power = 25;
int scan_power = 10;
int robot_rotate_radius = 39; // the distance from edge to sensor after rotating 180 deg when edge detected

// This is for the measure of going forward to edge.
int enc_value, enc_value_begin;
int fully_inside_table;

//////////////////////////
// Initial values
//////////////////////////
clearDebugStream();
setLEDColor(ledRed);
int start_message = 1;
int measure_complete = 0;
float distance[3];

while(true)
{
// Read the sensor
currentDistance = SensorValue[Color];


// This is a protection mechanism that will not start if at the begining the
// Roboot sensor is outsode of the table boundary
if (start_flag==1 & currentDistance < threshold)
{
displayCenteredBigTextLine(6, "Move me !!!");
multi_beep(5);
sleep(2000);
break;
} else if (start_flag == 1) { start_flag = 2; }



// Before doing any movement, we do a full 360deg scan to make sure that the robot is fully enclosed on the table boundary
if (start_flag == 2) {

fully_inside_table = scan_360(power,threshold);
waitForStopAndBeep();
if (fully_inside_table == 1) {
multi_beep(5);
sleep(2000);
break;
} else {start_flag = 0;}

datalogDataGroupStart();
datalogAddValue( DATALOG_SERIES_1, start_flag );
datalogAddValue( DATALOG_SERIES_2, fully_inside_table );
datalogAddValue( DATALOG_SERIES_4, 940 );
datalogDataGroupEnd();

}



// After emergency stop the robot will not move any more
if (emergency_stop==1) {
move_flag = 0;
}

// Continue to normanl operation only if the start_flag is turned off (means no falling risk)
if (move_flag == 0) {
motor[leftMotor] = 0;
motor[rightMotor] = 0;
} else if (start_flag==0 & currentDistance > threshold) {
if (start_message==1) { writeDebugStreamLine("Start Robot trip %d" , __DATE__); }
start_message = 0;

setMotorSync(leftMotor, rightMotor, 0, power);

sleep(1);
} else if (start_flag==0 & currentDistance < threshold) {

// stop
motor[leftMotor] = 0;
motor[rightMotor] = 0;
playTone(600, 40);

sleep (1000);

// Moving forward a bit to make sure that sensor is outside of table
setMotorSyncEncoder(leftMotor, rightMotor, 0, 75, 15);
// Move head to left until meeting table.
waitForStopAndBeep();
sleep(500);
//Move counterclockwise until meeting edge
move_until_enter_edge_from_outside(scan_power,threshold,-100);
sleep(500);
resetGyro(gyroSensor);
sleep(500);
resetGyro(gyroSensor);
sleep(200);
datalogDataGroupStart();
datalogAddValue( DATALOG_SERIES_1, getGyroDegrees(gyroSensor) );
datalogAddValue( DATALOG_SERIES_4, 996 );
datalogDataGroupEnd();

deg = scan_clockwise(scan_power, threshold);
resetGyro(gyroSensor);


sleep(500);
turn_degree = deg / 2;

datalogDataGroupStart();
datalogAddValue( DATALOG_SERIES_1, getGyroDegrees(gyroSensor) );
datalogAddValue( DATALOG_SERIES_2, deg );
datalogAddValue( DATALOG_SERIES_3, turn_degree );
datalogAddValue( DATALOG_SERIES_4, 999 );
datalogDataGroupEnd();

exactTurn(turn_degree,20,3000,-1,0);
sleep(500);
resetGyro(gyroSensor);
sleep(500);

// If degree is more than 180 than it means that we are on a corner.
// it is illegel state and we want to restasrt the flow.
if (deg >= 180) {
multi_beep (5);
writeDebugStreamLine("EV3 reached a risky situation on the corner of the table. Please manually move to a safer location and restart!");
sleep(2000);
break;
}

datalogDataGroupStart();
datalogAddValue( DATALOG_SERIES_1, getGyroDegrees(gyroSensor) );
datalogAddValue( DATALOG_SERIES_4, 990 );
datalogDataGroupEnd();
// Angle achieved after turn. We keep it since when we turn 180deg we want to use it as a reference.
//achieved_angle = getGyroDegrees(gyroSensor);
// Move backward until the color sdensor is on the boundary. This move can cause a little drift in angle that wil lbe fixed
// latyer on the 180 turn
move_until_enter_edge_from_outside(-scan_power/2,threshold,0);
sleep(500);
// We need to rotate 180 deg (from the original angle after exactTurn was running).
//drift = getGyroDegrees(gyroSensor) - achieved_angle;

datalogDataGroupStart();
datalogAddValue( DATALOG_SERIES_1, getGyroDegrees(gyroSensor) );
datalogAddValue( DATALOG_SERIES_4, 989 );
datalogDataGroupEnd();

rotate_back = 180 - getGyroDegrees(gyroSensor);
//resetGyro(gyroSensor);
//sleep(500);
exactTurn(rotate_back,20,3000,1,0);
sleep(500);

datalogDataGroupStart();
datalogAddValue( DATALOG_SERIES_1, getGyroDegrees(gyroSensor) );
datalogAddValue( DATALOG_SERIES_4, 988 );
datalogDataGroupEnd();

// Now going forward until reraching edge
// Encoder values pre-move
enc_value_begin = motor_enc_value();
move_until_enter_edge_from_inside(scan_power*2,threshold,0);
enc_value = motor_enc_value() ;
distance[measure_complete]= robot_rotate_radius + (wheel_diameter/10)*PI*(enc_value - enc_value_begin)/360 ;
// program will end when measure_complete=2
measure_complete ++;

// if first measure has been reached, we take the robot back a bit and turn 90 deg (no need to be accurate
// since it start a new roun of measure
if (measure_complete==1) {
motor[leftMotor] = 0;
motor[rightMotor] = 0;
setMotorSync(leftMotor, rightMotor, 0, -power);
sleep(1500);
motor[leftMotor] = 0;
motor[rightMotor] = 0;
waitForStopAndBeep();
exactTurn(90,20,3000,1,0);
sleep(500);
resetGyro(gyroSensor);
sleep(500);
}


if (measure_complete==2) {
playSoundFile("Cheering");
sleep(1000);
datalogDataGroupStart();
datalogAddValue( DATALOG_SERIES_2,distance[0] );
datalogAddValue( DATALOG_SERIES_3,distance[1] );
datalogAddValue( DATALOG_SERIES_5, 1000 );
datalogDataGroupEnd();

writeDebugStreamLine("Table dimentions as measured by EV3:");
writeDebugStreamLine("Size is: %d X %d", distance[0] , distance[1] );

break;
};

}

}

}


Fri Dec 09, 2016 6:54 am
Profile
Display posts from previous:  Sort by  
Reply to topic   [ 1 post ] 

Who is online

Users browsing this forum: No registered users and 2 guests


You cannot post new topics in this forum
You cannot reply to topics in this forum
You cannot edit your posts in this forum
You cannot delete your posts in this forum
You cannot post attachments in this forum

Search for:
Jump to:  



Powered by phpBB © 2000, 2002, 2005, 2007 phpBB Group.
Designed by ST Software for PTF.