#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.)
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)
}
}
Download PDF format
