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); } } } }