int main (const int argc, const char* argv[]) {
if (hasOption(argc, argv, "-h") || hasOption(argc, argv, "--help")) {
showHelp();
// Terminates the program after the help is shown.
return 0;
}
if (hasOption(argc, argv, "--verbose")) {
::demo::isVerbose = true;
}
// Initialises WiringPi and uses the BCM pin layout.
// For an overview on the pin layout, use the `gpio readall` command on a Raspberry Pi.
::wiringPiSetupGpio();
demo::ExtensionSensors extensionSensors(demo::Gpio::allocateSpi(), {0, 1, 2, 3, 4, 5}, 0.168, 0.268);
extensionSensors.setNumberOfSamplesPerMeasurment(3);
arma::Mat<double> extensionSensorsCorrection;
if (extensionSensorsCorrection.load("extensionSensors.correction")) {
std::cout << "Using the extension sensor correction." << std::endl;
extensionSensors.setMeasurementCorrections(extensionSensorsCorrection);
} else {
std::cout << "Could not find extension sensor correction file. Displaying uncorrected measurements." << std::endl;
}
std::vector<demo::Pin> directionPins;
directionPins.push_back(demo::Gpio::allocatePin(22));
directionPins.push_back(demo::Gpio::allocatePin(5));
directionPins.push_back(demo::Gpio::allocatePin(6));
directionPins.push_back(demo::Gpio::allocatePin(13));
directionPins.push_back(demo::Gpio::allocatePin(19));
directionPins.push_back(demo::Gpio::allocatePin(26));
demo::ServoControllers servoControllers(std::move(directionPins), demo::Gpio::allocateI2c(), {0, 1, 2, 3, 4, 5}, 1.0);
demo::LinearActuators linearActuators(std::move(servoControllers), std::move(extensionSensors), 0.178, 0.248);
linearActuators.setAcceptableExtensionDeviation(0.005);
demo::AttitudeSensors attitudeSensors(demo::Gpio::allocateUart(), -arma::datum::pi, arma::datum::pi);
arma::Mat<double>::fixed<3, 6> baseJointsPosition;
baseJointsPosition.load("baseJointsPosition.config");
arma::Mat<double>::fixed<3, 6> endEffectorJointsRelativePosition;
endEffectorJointsRelativePosition.load("endEffectorJointsRelativePosition.config");
demo::StewartPlatform stewartPlatform(std::move(linearActuators), std::move(attitudeSensors), baseJointsPosition, endEffectorJointsRelativePosition, {-0.02, -0.02, 0.21, -0.2, -0.2, -0.6}, {0.02, 0.02, 0.27, 0.2, 0.2, 0.6});
if (hasOption(argc, argv, "sensor")) {
runSensor(stewartPlatform);
} if (argc > 6 && isNumber(argv[1]) && isNumber(argv[2]) && isNumber(argv[3]) && isNumber(argv[4]) && isNumber(argv[5]) && isNumber(argv[6])) {
runEndEffectorPose(stewartPlatform, {std::stod(argv[1]), std::stod(argv[2]), std::stod(argv[3]), std::stod(argv[4]), std::stod(argv[5]), std::stod(argv[6])});
} else {
runDefault(stewartPlatform);
}
return 0;
}
int main(void)
{
WDTCTL = WDTPW | WDTHOLD; // Stop watchdog timer
// Disable the GPIO power-on default high-impedance mode
// to activate previously configured port settings
PMM_unlockLPM5();
//bools for running control logic for each switch
bool running = false;
bool running2 = false;
//initialise systems
init();
__enable_interrupt();
//stores data returned from colour sensor
struct colour detectedColour;
//stores MM decision from comparator
enum mmColours detectedMM;
//initialise counts to 0
numOfMMs.red = 0;
numOfMMs.orange = 0;
numOfMMs.yellow = 0;
numOfMMs.brown = 0;
numOfMMs.green = 0;
numOfMMs.blue = 0;
//number of mms sorted in current batch
int mmSorted = 0;
//main loop
while(1)
{
//switch 2 has interrupted
if (SW2_interruptFlag_)
{
//ignore input from button for 0.1s
__delay_cycles(100000);
SW2_interruptFlag_ = false;
//start sorting
running = true;
}
//switch 1 has interrupted
else if (SW1_interruptFlag_)
{
SW1_interruptFlag_ = false;
//update LCD with next value
running2 = true;
}
if (running2)
{
//display next mm count on the LCD
showNextCount();
updateLCDWithCount(numOfMMs);
running2 = false;
}
while (running)
{
//wait for an mm to enter seperator
__delay_cycles(500000);
//move seperator to colour sensor
changeServoADutyCycle(0x32);
//wait for mm to enter detection chamber
__delay_cycles(300000);
//run sensor
detectedColour = runSensor();
//compare sensor reading to calibration data
detectedMM = decideColour(detectedColour);
//increse count for detected MM by 1
incrementCount(detectedMM);
//run sorter
moveSorter(detectedMM);
__delay_cycles(1000000);
//move seperator to exit
changeServoADutyCycle(0x50);
//wait for mm to drop
__delay_cycles(700000);
//move seperator to entrance
changeServoADutyCycle(0x20);
//reset servo b
changeServoBDutyCycle(0x20);
__delay_cycles(1000000);
//sorted one more from current batch
mmSorted++;
if (mmSorted == 1)
{
running = false;
mmSorted = 0;
updateLCDWithCount(numOfMMs);
}
//stop if the button is pressed
if (SW2_interruptFlag_)
{
__delay_cycles(100000);
SW2_interruptFlag_ = false;
running = false;
mmSorted = 0;
updateLCDWithCount(numOfMMs);
}
}
}
}
