Commit f3c41f58 authored by Thomas Pryor's avatar Thomas Pryor
Browse files

Scnence Station update

parent bd1c075b
......@@ -23,7 +23,7 @@ void armRiseAndBack() {
armMotor[i].writeMicroseconds(SPEEDUP_FREQUENCY);
//Serial.println(TALON_ARM[i]);
delay(5000);
armMotor[i].writeMicroseconds(SPEEDDOWN_FREWQUENCY);
armMotor[i].writeMicroseconds(SPEEDDOWN_FREQUENCY);
delay(5000);
armMotor[i].writeMicroseconds(NEUTRAL_FREQUENCY);
}
......
import socket
UDP_IP = "192.168.1.121"
UDP_PORT = 8888
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
sock.bind((UDP_IP, UDP_PORT))
while True:
data, address = sock.recvfrom(1024)
print data
\ No newline at end of file
void getRoverGPS()
{
int packetSize = Udp.parsePacket();
Udp.read(packetBuffer, 96);
}
/*void initializeCompass()
{
compass.init();
compass.enableDefault();
compass.m_min = (LSM303::vector<int16_t>){-32767, -32767, -32767};
compass.m_max = (LSM303::vector<int16_t>){+32767, +32767, +32767};
Wire.begin();
//20 (SDA), 21 (SCL)
}
void updateCompass()
{
compass.read();
Udp.beginPacket(Udp.remoteIP(), Udp.remotePort());
Udp.write("Heading: "); // 0 = North, 90 = East, 180 = South, 270 = West
Udp.write(compass.heading());
Udp.endPacket();
}
*/
void initializeCompressor()
{
pinMode(COMPRESSOR_CONTROL, OUTPUT);
pinMode(COMPRESSOR_PIN, INPUT);
Serial.println("INITIALIZED");
}
void compressorPressureCheck()
{
int pressureValue = analogRead(COMPRESSOR_PIN) * 0.363;
//Serial.println(pressureValue);
if(pressureValue < 125) {
//pressureValue = 75 + 75 * sin(millis() / 1000.0) + random(0, 200) / 100.0;
// Serial.println(pressureValue);
// Need to use a bang bang with hysteresis band controller.
// This requires desired operating pressure, band, and previous state
int error = pressureValue - COMPRESSOR_PRESSURE;
// Negative error means pressure is low.
// State 0: Pressure is below hysteresis band
// State 1: Pressure is within band on upswing
// State 2: Pressure is above hysteresis band
// State 3: Pressure is within band on downswing
// Process flow is 0 -> 1 -> 2 -> 3 -> 0 -> ...
/*
Pressure vs Time Plot
Solid line - Desired Pressure (COMPRESSOR_PRESSURE)
Dashed line - Hysteresis band (COMPRESSOR_PRESSURE +- COMPRESSOR_BAND)
/--\ /-- State 2
_ _ _/ _ _\ _ _ _ _ _ _/ _ _ _ _ _ _ _ _ _ _
/ \ /
___/________\________/____ State 1 or 3
/ \ /
_/ _ _ _ _ _ _\ _ _/ _ _ _ _ _ _ _ _ _ _ _ _
/ \--/ State 0
*/
if (error < -COMPRESSOR_BAND){
// State 0
compressorStateOld = 0;
//Serial.println("State 0, Out of Band and Low Pressure");
digitalWrite(COMPRESSOR_CONTROL, HIGH);
armMotor[1].writeMicroseconds(1250);
} else if (error >= -COMPRESSOR_BAND && error <= COMPRESSOR_BAND){
// State 1 or 3
if (compressorStateOld == 0){
// State 1
digitalWrite(COMPRESSOR_CONTROL, HIGH);
}
else if(pressureValue > 165) {
//Serial.println("State 1, In Band on Upswing");
} else {
// State 3
digitalWrite(COMPRESSOR_CONTROL, LOW);
//Serial.println("State 3, In Band on Downswing");
}
} else if (error > COMPRESSOR_BAND){
// State 2
digitalWrite(COMPRESSOR_CONTROL, LOW);
compressorStateOld = 2;
//Serial.println("State 2, Out of Band and High Pressure");
} else {
Serial.println("How did I get here?");
}
}
......@@ -10,29 +10,12 @@
int currentReadingMax = 0;
int currentReadingMin = 169;
void getReadingFromCurrentSensor() {
// Data gathering
int currentReading = analogRead(CURRENT_SENSOR_PIN);
delay(500);
if (currentReading > currentReadingMax) {
currentReadingMax = currentReading;
}
if (currentReading < currentReadingMin) {
currentReadingMin = currentReading;
}
// Data translating to milliAmps
int current = rareDataToAmp(currentReading); // in milliAmps
int currentMax = rareDataToAmp(currentReadingMax);
int currentMin = rareDataToAmp(currentReadingMin);
printReading(current, currentMax, currentMin);
}
// Turn the rare analog read from Arduino to current reading
// @requires: an analog reading input from ACS758
// @throws: If input is smaller than 101 or larger than 189,
// throws error message in console
// @returns: a current value in milliAmps.
int rareDataToAmp(int analogReading) {
int rawDataToAmp(int analogReading) {
// From the Datasheet, we have 40mV/A, which equals to 25 mA/mV
int sensitivity = 25;
// Value measured, Apr. 11, 2015, Beck Pang & Andrew DeBartolo
......@@ -54,11 +37,28 @@ int rareDataToAmp(int analogReading) {
return voltageRead * sensitivity;
}
void overCurrentCheck()
{
int currentReading = analogRead(CURRENT_SENSOR_PIN);
int current = rawDataToAmp(currentReading);
if (current > DANGER_CURRENT) {
digitalWrite(ALARM_PIN, HIGH);
for (int i = 0; i <= 10; i++) {
Udp.beginPacket(Udp.remoteIP(), Udp.remotePort());
Udp.write("OVER CURRENT!!!!!!!!!");
Udp.endPacket();
}
for(int i = 0; i < 3; i++) {
armMotor[i].writeMicroseconds(NEUTRAL_FREQUENCY);
}
leftMotor.writeMicroseconds(NEUTRAL_FREQUENCY);
rightMotor.writeMicroseconds(NEUTRAL_FREQUENCY);
delay(15000);
}
}
void printReading(int current, int currentMax, int currentMin) {
Serial.print("A1 Reads current: ");
Serial.print(current);
......
......@@ -32,12 +32,13 @@ void receiveWirelessData()
void initializeWireless()
{ // begin wireless communication
//Serial.begin(BAUD_RATE);
Ethernet.begin(mac, ip);
Serial.begin(BAUD_RATE);
Ethernet.begin(mac);
//Ethernet.begin(mac, ip);
Udp.begin(UDP_PORT);
timeLastPacket = millis();
//Serial.println("INITIALIZED");
Serial.println("INITIALIZED");
// Serial.println("WRITTEN");
}
......
// Set GPSECHO to 'false' to turn off echoing the GPS data to the Serial console
// Set to 'true' if you want to debug and listen to the raw GPS sentences.
#define GPSECHO true
// this keeps track of whether we're using the interrupt
// off by default!
boolean usingInterrupt = false;
void useInterrupt(boolean); // Func prototype keeps Arduino 0023 happy
void initializeGPS()
{
// connect at 115200 so we can read the GPS fast enough and echo without dropping chars
// also spit it out
Serial.begin(115200);
Serial.println("Adafruit GPS library basic test!");
// 9600 NMEA is the default baud rate for Adafruit MTK GPS's- some use 4800
GPS.begin(9600);
// uncomment this line to turn on RMC (recommended minimum) and GGA (fix data) including altitude
GPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA);
// uncomment this line to turn on only the "minimum recommended" data
//GPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY);
// For parsing data, we don't suggest using anything but either RMC only or RMC+GGA since
// the parser doesn't care about other sentences at this time
// Set the update rate
GPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ); // 1 Hz update rate
// For the parsing code to work nicely and have time to sort thru the data, and
// print it out we don't suggest using anything higher than 1 Hz
// Request updates on antenna status, comment out to keep quiet
GPS.sendCommand(PGCMD_ANTENNA);
// the nice thing about this code is you can have a timer0 interrupt go off
// every 1 millisecond, and read data from the GPS for you. that makes the
// loop code a heck of a lot easier!
useInterrupt(true);
delay(1000);
// Ask for firmware version
Serial.println(PMTK_Q_RELEASE);
}
// Interrupt is called once a millisecond, looks for any new GPS data, and stores it
SIGNAL(TIMER0_COMPA_vect) {
char c = GPS.read();
// if you want to debug, this is a good time to do it!
#ifdef UDR0
if (GPSECHO)
if (c) UDR0 = c;
// writing direct to UDR0 is much much faster than Serial.print
// but only one character can be written at a time.
#endif
}
void useInterrupt(boolean v) {
if (v) {
// Timer0 is already used for millis() - we'll just interrupt somewhere
// in the middle and call the "Compare A" function above
OCR0A = 0xAF;
TIMSK0 |= _BV(OCIE0A);
usingInterrupt = true;
} else {
// do not call the interrupt function COMPA anymore
TIMSK0 &= ~_BV(OCIE0A);
usingInterrupt = false;
}
}
uint32_t timer = millis();
void getGPSLocation() // run over and over again
{
// in case you are not using the interrupt above, you'll
// need to 'hand query' the GPS, not suggested :(
if (! usingInterrupt) {
// read data from the GPS in the 'main loop'
char c = GPS.read();
// if you want to debug, this is a good time to do it!
if (GPSECHO)
if (c) Serial.print(c);
}
// if a sentence is received, we can check the checksum, parse it...
if (GPS.newNMEAreceived()) {
// a tricky thing here is if we print the NMEA sentence, or data
// we end up not listening and catching other sentences!
// so be very wary if using OUTPUT_ALLDATA and trytng to print out data
//Serial.println(GPS.lastNMEA()); // this also sets the newNMEAreceived() flag to false
if (!GPS.parse(GPS.lastNMEA())) // this also sets the newNMEAreceived() flag to false
return; // we can fail to parse a sentence in which case we should just wait for another
}
// if millis() or timer wraps around, we'll just reset it
if (timer > millis()) timer = millis();
// approximately every 2 seconds or so, print out the current stats
if (millis() - timer > 2000) {
timer = millis(); // reset the timer
/*Serial.print("\nTime: ");
Serial.print(GPS.hour, DEC); Serial.print(':');
Serial.print(GPS.minute, DEC); Serial.print(':');
Serial.print(GPS.seconds, DEC); Serial.print('.');
Serial.println(GPS.milliseconds);
Serial.print("Date: ");
Serial.print(GPS.day, DEC); Serial.print('/');
Serial.print(GPS.month, DEC); Serial.print("/20");
Serial.println(GPS.year, DEC);
Serial.print("Fix: "); Serial.print((int)GPS.fix);
Serial.print(" quality: "); Serial.println((int)GPS.fixquality);*/
if (GPS.fix) {
/*Serial.print("Location: ");
Serial.print(GPS.latitude, 4); Serial.print(GPS.lat);
Serial.print(", ");
Serial.print(GPS.longitude, 4); Serial.println(GPS.lon);
Serial.print("Location (in degrees, works with Google Maps): ");
Serial.print(GPS.latitudeDegrees, 4);
Serial.print(", ");
Serial.println(GPS.longitudeDegrees, 4);
Serial.print("Speed (knots): "); Serial.println(GPS.speed);
Serial.print("Angle: "); Serial.println(GPS.angle);
Serial.print("Altitude: "); Serial.println(GPS.altitude);
Serial.print("Satellites: "); Serial.println((int)GPS.satellites);*/
Serial.println("Sending GPS Location Over Udp...");
Udp.beginPacket(Udp.remoteIP(), Udp.remotePort());
Udp.write("Heading: "); // 0 = North, 90 = East, 180 = South, 270 = West
Udp.write(GPS.angle);
Udp.write("N ");
Udp.write(GPS.latitudeDegrees);
Udp.write("W ");
Udp.write(GPS.longitudeDegrees);
Udp.write("Speed: ");
Udp.write(GPS.speed * 1.15);
Udp.write("Altitude: ");
Udp.write(GPS.altitude);
Udp.endPacket();
Serial.println("GPS Data Sent");
}
}
}
......@@ -31,15 +31,15 @@ void writeToHandMotors()
if(hand[segments] == 0){
digitalWrite(GRIPPER[1], HIGH);
digitalWrite(GRIPPER[2], LOW);
Serial.println("Close");
// Serial.println("Close");
} else if (hand[segments] == 2){
digitalWrite(GRIPPER[1], LOW);
digitalWrite(GRIPPER[2], HIGH);
Serial.println("Open");
// Serial.println("Open");
} else {
digitalWrite(GRIPPER[1], LOW);
digitalWrite(GRIPPER[2], LOW);
Serial.println("Hold");
// Serial.println("Hold");
}
}
Adafruit-GPS-Library @ fb579ed5
Subproject commit fb579ed5810c1a64daf66f930dbe12a7fb1dddf0
#include <LSM303.h>
#include <Wire.h>
#include <math.h>
// Defines ////////////////////////////////////////////////////////////////
// The Arduino two-wire interface uses a 7-bit number for the address,
// and sets the last bit correctly based on reads and writes
#define D_SA0_HIGH_ADDRESS 0b0011101
#define D_SA0_LOW_ADDRESS 0b0011110
#define DLHC_DLM_DLH_MAG_ADDRESS 0b0011110
#define DLHC_DLM_DLH_ACC_SA0_HIGH_ADDRESS 0b0011001
#define DLM_DLH_ACC_SA0_LOW_ADDRESS 0b0011000
#define TEST_REG_ERROR -1
#define D_WHO_ID 0x49
#define DLM_WHO_ID 0x3C
// Constructors ////////////////////////////////////////////////////////////////
LSM303::LSM303(void)
{
/*
These values lead to an assumed magnetometer bias of 0.
Use the Calibrate example program to determine appropriate values
for your particular unit. The Heading example demonstrates how to
adjust these values in your own sketch.
*/
m_min = (LSM303::vector<int16_t>){-32767, -32767, -32767};
m_max = (LSM303::vector<int16_t>){+32767, +32767, +32767};
_device = device_auto;
io_timeout = 0; // 0 = no timeout
did_timeout = false;
}
// Public Methods //////////////////////////////////////////////////////////////
// Did a timeout occur in readAcc(), readMag(), or read() since the last call to timeoutOccurred()?
bool LSM303::timeoutOccurred()
{
bool tmp = did_timeout;
did_timeout = false;
return tmp;
}
void LSM303::setTimeout(unsigned int timeout)
{
io_timeout = timeout;
}
unsigned int LSM303::getTimeout()
{
return io_timeout;
}
bool LSM303::init(deviceType device, sa0State sa0)
{
// perform auto-detection unless device type and SA0 state were both specified
if (device == device_auto || sa0 == sa0_auto)
{
// check for LSM303D if device is unidentified or was specified to be this type
if (device == device_auto || device == device_D)
{
// check SA0 high address unless SA0 was specified to be low
if (sa0 != sa0_low && testReg(D_SA0_HIGH_ADDRESS, WHO_AM_I) == D_WHO_ID)
{
// device responds to address 0011101 with D ID; it's a D with SA0 high
device = device_D;
sa0 = sa0_high;
}
// check SA0 low address unless SA0 was specified to be high
else if (sa0 != sa0_high && testReg(D_SA0_LOW_ADDRESS, WHO_AM_I) == D_WHO_ID)
{
// device responds to address 0011110 with D ID; it's a D with SA0 low
device = device_D;
sa0 = sa0_low;
}
}
// check for LSM303DLHC, DLM, DLH if device is still unidentified or was specified to be one of these types
if (device == device_auto || device == device_DLHC || device == device_DLM || device == device_DLH)
{
// check SA0 high address unless SA0 was specified to be low
if (sa0 != sa0_low && testReg(DLHC_DLM_DLH_ACC_SA0_HIGH_ADDRESS, CTRL_REG1_A) != TEST_REG_ERROR)
{
// device responds to address 0011001; it's a DLHC, DLM with SA0 high, or DLH with SA0 high
sa0 = sa0_high;
if (device == device_auto)
{
// use magnetometer WHO_AM_I register to determine device type
//
// DLHC seems to respond to WHO_AM_I request the same way as DLM, even though this
// register isn't documented in its datasheet. Since the DLHC accelerometer address is the
// same as the DLM with SA0 high, but Pololu DLM boards pull SA0 low by default, we'll
// guess that a device whose accelerometer responds to the SA0 high address and whose
// magnetometer gives the DLM ID is actually a DLHC.
device = (testReg(DLHC_DLM_DLH_MAG_ADDRESS, WHO_AM_I_M) == DLM_WHO_ID) ? device_DLHC : device_DLH;
}
}
// check SA0 low address unless SA0 was specified to be high
else if (sa0 != sa0_high && testReg(DLM_DLH_ACC_SA0_LOW_ADDRESS, CTRL_REG1_A) != TEST_REG_ERROR)
{
// device responds to address 0011000; it's a DLM with SA0 low or DLH with SA0 low
sa0 = sa0_low;
if (device == device_auto)
{
// use magnetometer WHO_AM_I register to determine device type
device = (testReg(DLHC_DLM_DLH_MAG_ADDRESS, WHO_AM_I_M) == DLM_WHO_ID) ? device_DLM : device_DLH;
}
}
}
// make sure device and SA0 were successfully detected; otherwise, indicate failure
if (device == device_auto || sa0 == sa0_auto)
{
return false;
}
}
_device = device;
// set device addresses and translated register addresses
switch (device)
{
case device_D:
acc_address = mag_address = (sa0 == sa0_high) ? D_SA0_HIGH_ADDRESS : D_SA0_LOW_ADDRESS;
translated_regs[-OUT_X_L_M] = D_OUT_X_L_M;
translated_regs[-OUT_X_H_M] = D_OUT_X_H_M;
translated_regs[-OUT_Y_L_M] = D_OUT_Y_L_M;
translated_regs[-OUT_Y_H_M] = D_OUT_Y_H_M;
translated_regs[-OUT_Z_L_M] = D_OUT_Z_L_M;
translated_regs[-OUT_Z_H_M] = D_OUT_Z_H_M;
break;
case device_DLHC:
acc_address = DLHC_DLM_DLH_ACC_SA0_HIGH_ADDRESS; // DLHC doesn't have configurable SA0 but uses same acc address as DLM/DLH with SA0 high
mag_address = DLHC_DLM_DLH_MAG_ADDRESS;
translated_regs[-OUT_X_H_M] = DLHC_OUT_X_H_M;
translated_regs[-OUT_X_L_M] = DLHC_OUT_X_L_M;
translated_regs[-OUT_Y_H_M] = DLHC_OUT_Y_H_M;
translated_regs[-OUT_Y_L_M] = DLHC_OUT_Y_L_M;
translated_regs[-OUT_Z_H_M] = DLHC_OUT_Z_H_M;
translated_regs[-OUT_Z_L_M] = DLHC_OUT_Z_L_M;
break;
case device_DLM:
acc_address = (sa0 == sa0_high) ? DLHC_DLM_DLH_ACC_SA0_HIGH_ADDRESS : DLM_DLH_ACC_SA0_LOW_ADDRESS;
mag_address = DLHC_DLM_DLH_MAG_ADDRESS;
translated_regs[-OUT_X_H_M] = DLM_OUT_X_H_M;
translated_regs[-OUT_X_L_M] = DLM_OUT_X_L_M;
translated_regs[-OUT_Y_H_M] = DLM_OUT_Y_H_M;
translated_regs[-OUT_Y_L_M] = DLM_OUT_Y_L_M;
translated_regs[-OUT_Z_H_M] = DLM_OUT_Z_H_M;
translated_regs[-OUT_Z_L_M] = DLM_OUT_Z_L_M;
break;
case device_DLH:
acc_address = (sa0 == sa0_high) ? DLHC_DLM_DLH_ACC_SA0_HIGH_ADDRESS : DLM_DLH_ACC_SA0_LOW_ADDRESS;
mag_address = DLHC_DLM_DLH_MAG_ADDRESS;
translated_regs[-OUT_X_H_M] = DLH_OUT_X_H_M;
translated_regs[-OUT_X_L_M] = DLH_OUT_X_L_M;
translated_regs[-OUT_Y_H_M] = DLH_OUT_Y_H_M;
translated_regs[-OUT_Y_L_M] = DLH_OUT_Y_L_M;
translated_regs[-OUT_Z_H_M] = DLH_OUT_Z_H_M;
translated_regs[-OUT_Z_L_M] = DLH_OUT_Z_L_M;
break;
}
return true;
}
/*
Enables the LSM303's accelerometer and magnetometer. Also:
- Sets sensor full scales (gain) to default power-on values, which are
+/- 2 g for accelerometer and +/- 1.3 gauss for magnetometer
(+/- 4 gauss on LSM303D).
- Selects 50 Hz ODR (output data rate) for accelerometer and 7.5 Hz
ODR for magnetometer (6.25 Hz on LSM303D). (These are the ODR
settings for which the electrical characteristics are specified in
the datasheets.)
- Enables high resolution modes (if available).
Note that this function will also reset other settings controlled by
the registers it writes to.
*/
void LSM303::enableDefault(void)
{
if (_device == device_D)
{
// Accelerometer
// 0x00 = 0b00000000
// AFS = 0 (+/- 2 g full scale)
writeReg(CTRL2, 0x00);
// 0x57 = 0b01010111
// AODR = 0101 (50 Hz ODR); AZEN = AYEN = AXEN = 1 (all axes enabled)
writeReg(CTRL1, 0x57);
// Magnetometer
// 0x64 = 0b01100100
// M_RES = 11 (high resolution mode); M_ODR = 001 (6.25 Hz ODR)
writeReg(CTRL5, 0x64);
// 0x20 = 0b00100000
// MFS = 01 (+/- 4 gauss full scale)
writeReg(CTRL6, 0x20);
// 0x00 = 0b00000000
// MLP = 0 (low power mode off); MD = 00 (continuous-conversion mode)
writeReg(CTRL7, 0x00);
}
else
{
// Accelerometer
if (_device == device_DLHC)
{
// 0x08 = 0b00001000
// FS = 00 (+/- 2 g full scale); HR = 1 (high resolution enable)
writeAccReg(CTRL_REG4_A, 0x08);
// 0x47 = 0b01000111
// ODR = 0100 (50 Hz ODR); LPen = 0 (normal mode); Zen = Yen = Xen = 1 (all axes enabled)
writeAccReg(CTRL_REG1_A, 0x47);
}
else // DLM, DLH
{
// 0x00 = 0b00000000
// FS = 00 (+/- 2 g full scale)
writeAccReg(CTRL_REG4_A, 0x00);
// 0x27 = 0b00100111