Blackbox Detection Craft

main.ino

// The University of Queensland
// Engineering - ENGG1100
// White Team
// Year: 2015
// Author: Dragan Marjanovic


// Libraries and Headers
#include <Servo.h>
#include <Wire.h> //I2C Arduino Library
#define address 0x1E //0011110b, I2C 7bit address of HMC5883


// Global Constants and Bindings:

    // Motor Shield PWM Control:
    const int E1 = 5;
    const int M1 = 4;
    const int E2 = 6;
    const int M2 = 7;

    // Bump Switches:
    const int fls = 3;              // Front-Left Switch
    const int frs = 3;              // Front-Right Switch
    const int ps = 3;               // Plate Switch

    // Magnetometer - Expected value ranges:
    int ZRangeP[2] = {550,650};     // Positive Pole
    int ZRangeN[2] = {550,650};     // Negative Pole

    // Signalling LEDs
    const int ledPin =  13;         // Blackbox detected LED
        
    // Servo
    Servo servo1;




int buttonState = 0;         // variable for reading the pushbutton status


// Lower level functions

int percent_to_value(int percent){
    /* Takes percentage value and returns it as motor value.
    In: int(0-100)
    percent_to_value(int) -> int
    */

    int motor_max_speed = 255;
    int speed;
    const int sto = 100;
    switch (percent) {
        case speed > sto:
            speed = motor_max_speed;
            break;
        case speed < sto:
            speed = 0;
            break;
        default:
            // float decimal_percentage = (float)a / (float)b;
            break;
    }

    return speed;
}



void winch_deploy(){
    servo1.attach(9);
    // Sets servo to 0 degrees
    servo1.write(180);
    // Delays servo for 5000 ms
    delay(5000);
    servo1.write(0);
    delay(5000);
    // Ends communication with the servo
    servo1.detach();
}
 
 
void motor_tester(){
    digitalWrite(M1,HIGH);   
    digitalWrite(M2, HIGH);       
    analogWrite(E1, 255);   //PWM Speed Control
    analogWrite(E2, 255);   //PWM Speed Control
}


void magnetometer(int ZRange[]){
    int x,y,z; //triple axis data

    //Tell the HMC5883 where to begin reading data
    Wire.beginTransmission(address);
    Wire.write(0x03); //select register 3, X MSB register
    Wire.endTransmission();


    //Read data from each axis, 2 registers per axis
    Wire.requestFrom(address, 6);
    if(6<=Wire.available()){
        x = Wire.read()<<8; //X msb
        x |= Wire.read(); //X lsb
        z = Wire.read()<<8; //Z msb
        z |= Wire.read(); //Z lsb
        y = Wire.read()<<8; //Y msb
        y |= Wire.read(); //Y lsb
    }

    //Print out values of each axis
    Serial.print("x: ");
    Serial.print(x); 
    Serial.print("  y: ");
    Serial.print(y);
    Serial.print("  z: ");
    Serial.println(z);
    if (z >= ZRange[0] && z <= ZRange[1] ){
        winch_deploy();
    }

    delay(10);
} 


// Handler Functions








void setup() { 
    // Sets up PIN 9
    pinMode(9,OUTPUT);
    // Connects to Servo


    pinMode(M1, OUTPUT);   
    pinMode(M2, OUTPUT); 
  
  // pinMode(buttonPin, INPUT);     
  pinMode(ledPin, OUTPUT); 
  
    //Initialize Serial and I2C communications
    Serial.begin(9600);
    Wire.begin();

    //Put the HMC5883 IC into the correct operating mode
    Wire.beginTransmission(address); //open communication with HMC5883
    Wire.write(0x02); //select mode register
    Wire.write(0x00); //continuous measurement mode
    Wire.endTransmission();
    
}
 

// Main Loop and Control Function  
void loop() {
  // buttonState = digitalRead(buttonPin);
  
  // if (digitalRead(buttonPin) == HIGH) {     
  //   // turn LED on:    
  //   digitalWrite(ledPin, HIGH);  
  // } 
  // else {
  //   // turn LED off:
  //   digitalWrite(ledPin, LOW); 
  // }
  
  Serial.println(percent_to_value(100));

}