RobotC: Autonomous Holonomic Platform using 4x Sonars

Killough & Holonomic Platforms LEGO Mindstorms Videos My NXT Robots NXT Program Code NXT Ultrasonic Sensor RobotC: Code Rotacaster Omni-Wheels Standard NXT Motors Published July 9, 2012 at 7:26 pm 3 Comments

This Autonomous Tri-Drive Holonomic Platform is fitted with Rotacaster® Omni-wheels and uses 4x Ultrasonic Sensors for navigation. The RobotC Code is based on my previous post on using multiple Sonar Sensors in RobotC. The main challenge is to use all 4 Ultrasonic Sensors without them interfering with each other, resulting in non-valid measurement readings for distance values to obstacles.

Tri-Holomonic Platform with Sonar Navigation
Autonomous Holonomic Platform using 4x Sonars for Navigation

If you wish to use Multiple Ultrasonic Sensors on your NXT Robot, you need to program them so only ONE Sensor is taking a measurement at any  given time. As  mentioned above, if you have all the Ultrasonic Sensors on at once they interfere with each other and give incorrect readings. To get around this you need to take advantage of the Sensors Mode 1: Single Shot Mode. Refer to the Lego Mindstorms NXT Hardware Developer Kit, Appendix 7: LEGO MINDSTORMS NXT Ultrasonic Sensor I2C communication protocol for details. Consult the RobotC API for detailed information on using Digital I2C Command & Control with NXT Sensors.


Autonomous Holonomic Platform using 4x Sonars for Navigation

The video above shows the Robot randomly moving around the walled area. The Robot is programmed to moves no closer to the Walls than 40cm. After each encounter, a random direction away from the corresponding wall/s is generated. The “#define THRESHOLD 40” statement in the code below, controls the minimum distance to wall value the Robot uses.

Third Party Driver Suite:
The standard RobotC comes with a lot of built in drivers for a wide variety of sensors. However, the standard RobotC doesn’t expose all of the functions that some of the sensors have available. To use the Sonar Sensor in ‘Single Shot Mode’ with RobotC, you will need a copy of Xander Soldaat’s ‘Third Party Driver Suite‘.

I also so suggest you download a copy of Xander Soldaat’s ‘RobotC Third Party Driver Suite Tutorial‘ which is an excellent resource. For more information relating to Xander Soldaat’s  ‘Third Party Driver Suite‘, visit his blog, ‘Bot Bench‘.

RobotC Source Code:

Download RobotC Source Code: TriHolonomicSonar.C

#pragma config(Sensor, S1,      US1,          sensorI2CCustom9V)
#pragma config(Sensor, S2,      US2,          sensorI2CCustom9V)
#pragma config(Sensor, S3,      US3,          sensorI2CCustom9V)
#pragma config(Sensor, S4,      US4,          sensorI2CCustom9V)
#pragma config(Motor,  motorA,  tmotorNormal,  tmotorNormal, openLoop)
#pragma config(Motor,  motorB,  tmotorNormal,  tmotorNormal, openLoop, reversed)
#pragma config(Motor,  motorC,  tmotorNormal,  tmotorNormal, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/*
	          Original Code by Ray McNamara (www.rjmcnamara.com)

This program is free software: you can redistribute it and/or modify it under the terms
of the GNU General Public License as published by the Free Software Foundation, either
version 3 of the License, or (at your option) any later version. In particular, you must
report the name of the original author.

This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
See the GNU General Public License for more details.

You can get a copy of the GNU General Public License from www.gnu.org.

*/

/**********************************************************************************
*              Set Constants and Variables                                        *
**********************************************************************************/
#include "drivers/LEGOUS-driver.h"

#define THRESHOLD 40

float  V1, V2, V3;
int    Vx, Vy, PowerA, PowerB, PowerC;
int    Dist1, Dist2, Dist3, Dist4;

/**********************************************************************************
*              Monitor the 3x Utrasonic Sonar Sensors                             *
***********************************************************************************/
task MonitorUS()
{
  while (true)
  {
    nxtDisplayClearTextLine(4);
    USsetSingleMode(US1);                 // Set US1 Sensor in Single Shot Mode
    wait1Msec(50);                        // Wait to Capture Distance Value
    Dist1 = USreadDist(US1);              // Read the Sonar Sensor's Distance Value
    nxtDisplayTextLine(4, "Sonar 1: %d", Dist1);
    USsetOff(US1);                        // Turn off Sonar Sensor on Digital ports 1

    nxtDisplayClearTextLine(5);
    USsetSingleMode(US2);                 // Set US1 Sensor in Single Shot Mode
    wait1Msec(50);                        // Wait to Capture Distance Value
    Dist2 = USreadDist(US2);              // Read the Sonar Sensor's Distance Value
    nxtDisplayTextLine(5, "Sonar 2: %d", Dist2);
    USsetOff(US1);                        // Turn off Sonar Sensor on Digital ports 1

    nxtDisplayClearTextLine(6);
    USsetSingleMode(US3);                 // Set US2 Sensor in Single Shot Mode
    wait1Msec(50);                        // Wait to Capture Distance Value
    Dist3 = USreadDist(US3);              // Read the Sonar Sensor's Distance Value
    nxtDisplayTextLine(6, "Sonar 3: %d", Dist3);
    USsetOff(US3);                        // Turn off Sonar Sensor on Digital ports 2

    nxtDisplayClearTextLine(7);
    USsetSingleMode(US4);                 // Set US3 Sensor in Single Shot Mode
    wait1Msec(50);                        // Wait to Capture Distance Value
    Dist4 = USreadDist(US4);              // Read the Sonar Sensor's Distance Value
    nxtDisplayTextLine(7, "Sonar 4: %d", Dist4);
    USsetOff(US4);                        // Turn off Sonar Sensor on Digital ports 3    

    // Sonar 1
    if (Dist1 < THRESHOLD && Dist2 > THRESHOLD && Dist3 > THRESHOLD && Dist4 > THRESHOLD)
		  { 
				Vx = -random(70)-30;  // Object Found, Change Vx Direction
		  }

    // Sonar 2
    if (Dist1 > THRESHOLD && Dist2 < THRESHOLD && Dist3 > THRESHOLD && Dist4 > THRESHOLD)
		  {
				Vx = random(70)+30;  // Object Found, Change Vx Direction
		  }

    // Sonar 3
    if (Dist1 > THRESHOLD && Dist2 > THRESHOLD && Dist3 < THRESHOLD && Dist4 > THRESHOLD)
		  { 
				Vy = random(70)+30;  // Object Found, Change Vy Direction
		  }

    // Sonar 4
    if (Dist1 > THRESHOLD && Dist2 > THRESHOLD && Dist3 > THRESHOLD && Dist4 < THRESHOLD)
		  { 
				Vy = -random(70)-30;  // Object Found, Change Vy Direction
		  }

    // Sonar 1 & 3
    if (Dist1 < THRESHOLD && Dist2 > THRESHOLD && Dist3 < THRESHOLD && Dist4 > THRESHOLD)
		  { 
				Vx = -random(70)-30;  // Object Found, Change Vx Direction
				Vy = random(70)+30;   // Object Found, Change Vy Direction
		  }

    // Sonar 1 & Sonar 4
    if (Dist1 < THRESHOLD && Dist2 > THRESHOLD && Dist3 > THRESHOLD && Dist4 < THRESHOLD)
		  { 
				Vx = -random(70)-30;  // Object Found, Change Vx Direction
				Vy = -random(70)-30;  // Object Found, Change Vy Direction
		  }

    // Sonar 2 & Sonar 3
    if (Dist1 > THRESHOLD && Dist2 < THRESHOLD && Dist3 < THRESHOLD && Dist4 > THRESHOLD)
		  { 
				Vx = random(70)+30;  // Object Found, Change Vx Direction
				Vy = random(70)+30;  // Object Found, Change Vy Direction
		  }

    // Sonar 2 & Sonar 4
    if (Dist1 > THRESHOLD && Dist2 < THRESHOLD && Dist3 > THRESHOLD && Dist4 < THRESHOLD)
		  { 
				Vx = random(70)+30;  // Object Found, Change Vx Direction
				Vy = -random(70)-30; // Object Found, Change Vy Direction
		  }
  /*    nxtDisplayClearTextLine(3);
    nxtDisplayTextLine(3, "Vx:%d Vy:%d", Vy, Vx);  // Display 'X' & 'Y' Co-ordinates  */

    Dist1 = 0; Dist2 = 0; Dist3 = 0; Dist4 = 0;
    wait1Msec(50);
   }
}

/**********************************************************************************
*              Main Killough Platform Control                                     *
***********************************************************************************/
task main()
{
  eraseDisplay();
  nxtDisplayCenteredBigTextLine(0, "Killough");
  nxtDisplayCenteredBigTextLine(2, "Platform");

  StartTask(MonitorUS);                     // Start Monitoring the Ultrasonic Sonar Sensors

  Vx = 100;
  Vy = 100;

  while (true)
  {
    V1 = Vx;                                // Vector Calculation for MotorA(V1)'s Power

    V2 = -Vx / 2 - sqrt(3)/2 * Vy;          // Vector Calculation for MotorB(V2)'s Power

    V3 = -Vx / 2 + sqrt(3)/2 * Vy;          // Vector Calculation for MotorC(V3)'s Power

    if (V1 < 30 && V1 > 0) {V1 = 30;}       // Set Minimum MotorA's Forward Power
    if (V1 < 0 && V1 > -30) {V1 = -30;}     // Set Minimum MotorA's Reverse Power
    if (V2 < 30 && V2 > 0) {V2 = 30;}       // Set Minimum MotorB's Forward Power
    if (V2 < 0 && V2 > -30) {V2 = -30;}     // Set Minimum MotorB's Reverse Power
    if (V3 < 30 && V3 > 0) {V3 = 30;}       // Set Minimum MotorC's Forward Power
    if (V3 < 0 && V3 > -30) {V3 = -30;}     // Set Minimum MotorC's Reverse Power

    PowerA = V2;                            // Convert to an Integer Value for MotorA
    PowerB = V1;                            // Convert to an Integer Value for MotorB
    PowerC = V3;                            // Convert to an Integer Value for MotorC

    motor[motorA] = PowerA;                 // Set MotorA's Velocity (Power Setting)
    motor[motorB] = PowerB;                 // Set MotorB's Velocity (Power Setting)
    motor[motorC] = PowerC;                 // Set MotorC's Velocity (Power Setting)
  }
}

 

 

Buy Rotacaster Omni-wheels

GD Star Rating
loading...
GD Star Rating
loading...
RobotC: Autonomous Holonomic Platform using 4x Sonars, 9.2 out of 10 based on 5 ratings
Link to this post!

Tags: , , , , , , , , , , , , , , , , , , , , ,

Subscribe without commenting

The Best of the Best, in LEGO Mindstorms NXT Publications for Your Bookshelf!

 

Free counters!