Project Goals

The goal of the project was to create a device that could fire small
projectiles into three seperate containers. 

Mechanical limitations:
Built from lego, including motors and sensors	
Must shoot at least two meters	
Minimum scanning area of 180 degrees	
Find up to three seperate targets and gauge the distance and torque required	
Before firing, user confirmation is necessary

Software limitations:	
C programming language with husarion/lego libraries	
The microcontroller is a husarion RoboCore	
Must take the input from the sensor and decide where potential targets lie	
Calculate required torque for a given distance
Three operating modes(Auto, Manual and Calibration)
Design
The robot is made up of 3 motors, 1 ultrasonic sensor, 1 press button and 1 RoboCORE. Two motors are used in tandem to increase the firing torque, which helps with the precision of shots as well as increasing
the range. The third motor provides the rotation of the firing lever
and scanner.
The button serves two purposes. The first is as an endstop. The endstop is used to set the zero position during initialization of the
catapult and to later recalibrate the printer as we found the motors
can be unreliable. The second function is as a user confirmation
before tasks such as firing projectiles that can result in serious
injury or even death; use of plastic balls is strongly advised.
To prevent damage to the pieces during firing, rubber dampers are
provided where there is contact.

Auto Mode
The auto mode includes a 180deg scan before deciding how many boxes there before and firing to each with user confirmation.
Manual Mode
The user inputs the angle of shot and distance of target. The rest is
computed by the robot.
Calibration Mode
Allows easy calibration of most parameters.




Below you will found the code.

#include <stdio.h>
#include <hFramework.h>
#include <math.h>
#include <Lego_Ultrasonic.h>
#include "Lego_Touch.h"

#define iMeas 190
#define tick 5
#define maxSens 225         //Maximum distance of target
#define goldilock 5         //Range of distance of target
#define minLen 5            //Minimum number of measurements to count as target

void calibrationMode();
void autoMode();
void manualControl();
void setLever();
void setAbs();
void proximityScan();
void systemScan();
void targetingAndFire();
void calibrate();
void calibrationPhase();
void fire(int);
void confirmPress();
void rotateAngle(int);
int setPower(int);

using namespace hSensors;

Lego_Ultrasonic sens(hSens1);
Lego_Touch sensor(hSens2);

int dist[iMeas];
int box[4]={0,0,0,0};

void hMain(void){
    printf("\r\n\n------Lady Fist Fire is getting ready, please hold------\r\n\n");
    calibrate();
    setAbs();
    printf("\r\nWe are ready to get going, what mode would you like to enter?");

    for (;;){
        if (hBtn1.isPressed() == true){
            sys.delay(500);
            if (hBtn2.isPressed() == true) {calibrationMode();}
            else {manualControl();}
        }
        if (hBtn2.isPressed() == true)  {autoMode();}
    }
}

void setLever(){
    hMot2.setPower(-200);
    hMot3.setPower(-200);
    sys.delay(180);
    hMot2.setPower(0);
    hMot3.setPower(0);
}

void proximityScan(){
    printf("\r\n\n*****Starting Scan*****\r\n\n");
    for (int i=0; i<iMeas; i++){
        hMot6.rotRel(tick,600);
        sys.delay(100);
        dist[i] = sens.readDist();
        printf("%d  |   ", dist[i]);
    }
    printf("\r\n\n*****Scan Finished*****\r\n\n");
}

//Calculate potential boxes and parameters
void systemScan(){
    printf("\r\n\n*****Starting Calculations*****\r\n\n");
    int n = 1;                   //Iterator
    int k = 0;                   //Iterator
    for (int j=0; j<iMeas; j++){
        if((dist[j] < maxSens)&&(abs(dist[j] - dist[j+1]) < goldilock)){
            k=1;
            printf("Found Potential Box");

            while(abs(dist[j+k] - dist[j+k+1]) < goldilock){k++;}
            printf("\r\nLength(k) = %d, j = %d\r\n",k,j);

            if(k>minLen){
                box[n]= (k/2)+j;
                n++;
            }
            j += k;
        }

    }
    printf("\r\n\n*****Calculations finished*****\r\n\n");
}

int setPower(int dist){
    int pow = 0;

    if (dist<=10){pow = 450;}
    else if (dist<=20){pow = 480;}
    else if (dist<=30){pow = 485;}
    else if (dist<=40){pow = 520;}
    else if (dist<=50)...