Code: Select all
/*
Drive one servomotor in response to potentiometer while sensing and logging current
The circuit:
Pin 0:
Pin 1:
Pin 2:
Pin 3:
Pin 4:
Pin 5: to Servo motor
Pin 6:
Pin 7:
Pin 8:
Pin 9:
Pin 10: CS (SD card)
Pin 11: DI (SD card)
Pin 12: DO (SD card)
Pin 13: CLK (SD card)
Pin A0: high side of current sensing resistor
Pin A1: low side of current sensing resistor
Pin A2:
Pin A3:
Pin A4:
Pin A5: potentiometer
other:
current sensing resistor between motor's black wire and ground
servomotor red wire to 5V arduino power
GND and COM on SD breakout connected --> common GND
VCC on SD card --> 3.3V
created 19 July 2011
by Dustyn Roberts
*/
#include <SD.h>
#define WAIT_TO_START 0 // Wait for serial input in setup()
#define ECHO_TO_SERIAL 1 // echo data to serial port
#define DATA_LOGGING 1 // whether or not to use SD card data logging code
#define PROCESSING 0 // whether or not to spit numbers out to graph in Processing
//motor stuff
int servoPin = 5; // digital pins for servo motor control
int minPulse = 600; // Minimum servo position
int maxPulse = 2400; // Maximum servo position
int pulse = 1500; // stores amount to pulse servo. set to neutral for safety
long lastPulse = 0; // the time in milliseconds of the last pulse on each servo
int refreshTime = 20; // the time needed in between pulses
int potPin = 5; // the analog pins that the pot is on
int potValue = 0; // the value returned from the pots
// current sensor stuff
float voltperunit = 5.0/1024.0; //5 Volts for Arduino power divided by 1024 bit resolution gives volts per unit
float resistance = 1.2; //ohms (of current sensing resistor)
float torque_constant = 0.024444499; // ?? for HS-645MG servomotor
int high_pin = 0; //high side of current sensing resistor
int high_read = 0;
int low_pin = 1; //low side of current sensing resistor
int low_read = 0;
int reading_diff = 0;
float voltage_diff = 0; //in Volts
float current = 0; //in Amps
//data logging stuff
const int chipSelect = 10; // 8 in Tom's Github code, 4 in his Arduino code for Ethernet shield, 10 on most Arduino boards
File logfile;
void error(char *str)
{
Serial.print("error: ");
Serial.println(str);
while(1);
}
void setup() {
Serial.begin (9600); // initialize serial communications:
Serial.println();
#if WAIT_TO_START
Serial.println("Type any character to start");
while (!Serial.available());
#endif //WAIT_TO_START
// MOTOR STUFF
pinMode(servoPin, OUTPUT); // Set each servo pin as an output pin
#if DATA_LOGGING
// make sure that the default chip select pin is set to output, even if you don't use it:
pinMode(10, OUTPUT);
// see if the card is present and can be initialized:
Serial.print("Initializing SD card...");
if (!SD.begin(chipSelect)) {
Serial.println(" card failed, or not present.");
return; // don't do anything more:
}
Serial.println("card initialized.");
// create a new file
char filename[] = "LOGGER00.txt";
for (uint8_t i = 0; i < 100; i++) {
filename[6] = i/10 + '0';
filename[7] = i%10 + '0';
if (! SD.exists(filename)) {
// only open a new file if it doesn't exist
logfile = SD.open(filename, FILE_WRITE);
break; // leave the loop!
}
}
if (! logfile) {
error("couldnt create file");
}
Serial.print("Logging to: ");
Serial.println(filename);
logfile.println("millis,high_read,low_read,current");
#if ECHO_TO_SERIAL
Serial.println("millis,high_read,low_read,current");
#endif // ECHO_TO_SERIAL
#endif //DATA_LOGGING
}
void loop() {
// MOTOR STUFF
potValue = analogRead(potPin); // read the analog input
pulse = map(potValue,0,1023,minPulse,maxPulse); // convert the analog value to a range between minPulse and maxPulse.
// pulse the servo again if rhe refresh time (20 ms) has passed:
if (millis() - lastPulse >= refreshTime) {
digitalWrite(servoPin, HIGH); // Turn the motor on
delayMicroseconds(pulse); // Length of the pulse sets the motor position
digitalWrite(servoPin, LOW); // Turn the motor off
lastPulse = millis(); // save the time of the last pulse
}
//CURRENT SENSOR STUFF
high_read = analogRead(high_pin);
low_read = analogRead(low_pin);
reading_diff = high_read-low_read;
voltage_diff = (float)reading_diff * voltperunit;
current = voltage_diff / resistance;
#if ECHO_TO_SERIAL
Serial.print(millis());
Serial.print(", ");
Serial.print(high_read);
Serial.print(", ");
Serial.print(low_read);
Serial.print(", ");
Serial.print(reading_diff);
Serial.print(", ");
Serial.println(current);
#endif // ECHO_TO_SERIAL
#if PROCESSING
Serial.print(reading_diff, BYTE);
#endif // PROCESSING
#if DATA_LOGGING
logfile.print(millis());
logfile.print(",");
logfile.print(high_read);
logfile.print(",");
logfile.print(low_read);
logfile.print(",");
logfile.print(reading_diff);
logfile.print(",");
logfile.println(current);
#endif // DATA_LOGGING
}