MEGR_3092 Hammer Down

SYSTEM_THREAD(ENABLED);

// Define the analog input pins
const int ANALOG_IN_CURRENT = A0;
const int ANALOG_IN_VOLTAGE = A1;
const int THROTTLE_PIN = A2;
const int DIRECTION_PIN = D2;

//Define outputs for speed controllers
int escOut = 1500;
Servo ESC;

// Define the voltage divider ratio
const float VOLTAGE_DIVIDER_RATIO = 3.0;

// Define the filter constants
const float FILTER_ALPHA = 0.1;
const int FILTER_SAMPLES = 500;

// Define the publishing interval
const int PUBLISH_INTERVAL_MS = 10000;

// Define the variable throttle values
double THROTTLE_RATE = 4;
double TARGET_THROTTLE;
int PREVIOUS_THROTTLE = 0;

// Define the raw variables
float rawCurrent = 0.0;
float rawVoltage = 0.0;
int rawThrottle = 0.0;

// Define the actual variables
float voltage = 0.0;
float current = 0.0;

// Declare variables for summing current and voltage values
float sumCurrent = 0.0;
float sumVoltage = 0.0;

// Declare the filtered variables
float filteredCurrent = 0.0;
float filteredVoltage = 0.0;

// Declare variable for motor speed
int motorSpeed = 0;

// Declare the filter buffers
float currentBuffer[FILTER_SAMPLES];
float voltageBuffer[FILTER_SAMPLES];
int bufferIndex = 0;

// Declare the last publish time
unsigned long lastPublishTime = 0;

// Declare the Particle variables and zero them out
double particleCurrent = 0.0;
double particleVoltage = 0.0;
int particleMotorSpeed = 0;

void setup() {
    // Set the analog input pins as inputs
    pinMode(ANALOG_IN_CURRENT, INPUT);
    pinMode(ANALOG_IN_VOLTAGE, INPUT);
    pinMode(THROTTLE_PIN, INPUT);
    pinMode(DIRECTION_PIN, INPUT_PULLDOWN);
    
    // Set up servo for speed controller
    ESC.attach(A3);
    

    // Set up Serial for debugging
    Serial.begin(9600);
    
    // Declare the Particle variables and zero them out
    Particle.variable("filteredCurrent", &particleCurrent, DOUBLE);
    Particle.variable("filteredVoltage", &particleVoltage, DOUBLE);
    Particle.variable("motorSpeed", &particleMotorSpeed, INT);
    
 
}

void loop() {
    // Read the analog inputs at 50Hz
    unsigned long currentTime = millis();
    static unsigned long lastReadTime = 0;
    if (currentTime - lastReadTime >= 20) {
        lastReadTime = currentTime;

        // Read the analog inputs
        rawCurrent = analogRead(ANALOG_IN_CURRENT);
        rawVoltage = analogRead(ANALOG_IN_VOLTAGE);
        rawThrottle = analogRead(THROTTLE_PIN);
    
        // Convert the raw values to actual values
        voltage = rawVoltage / 4095.0 * 3.3 * VOLTAGE_DIVIDER_RATIO;
        current = ((rawCurrent / 4095.0 * 3.3) - 1.65) / 0.066;

        // Update the filter buffers
        currentBuffer[bufferIndex] = current;
        voltageBuffer[bufferIndex] = voltage;
        bufferIndex = (bufferIndex + 1) % FILTER_SAMPLES;

        // Calculate the filtered values
        sumCurrent = 0.0;
        sumVoltage = 0.0;
        
            for (int i = 0; i < FILTER_SAMPLES; i++) {
            sumCurrent += currentBuffer[i];
            sumVoltage += voltageBuffer[i];
            }
            
        filteredCurrent = (sumCurrent / FILTER_SAMPLES * -1) - 2;
        filteredVoltage = sumVoltage / FILTER_SAMPLES + 3;
        

		motorSpeed = map(rawThrottle, 1100, 3215, 0, 255); //map the raw throttle values from 0 to 255
		motorSpeed = constrain(motorSpeed, 0, 255); //constrain the values to the range to prevent overflow or underflow
		
		TARGET_THROTTLE = motorSpeed;  //set the target value for slew rate throttle control
		
		if (PREVIOUS_THROTTLE < TARGET_THROTTLE){    //if the previous throttle is less than the target throttle
                PREVIOUS_THROTTLE += THROTTLE_RATE;  //increase the previous throttle by the throttle rate amount
                
        }
        else if (PREVIOUS_THROTTLE > TARGET_THROTTLE){ //if the previous throttle is larger than the target throttle (i.e. throttle has decreased)
            PREVIOUS_THROTTLE = TARGET_THROTTLE; //automatically set the throttle value to the target throttle
        }
	
    		
        if (digitalRead(DIRECTION_PIN) == LOW) {      //if the direction is forward
            escOut = map(PREVIOUS_THROTTLE, 0, 255, 1500, 2000);  //write the esc's from 1500 to 2000
    		ESC.writeMicroseconds(escOut);                        //write the value
    	} 
    	
    	else { 
    		escOut = map(PREVIOUS_THROTTLE, 0, 255, 1500, 1000);  //if the direction is reverse map the values from 1500 to 1000
    		ESC.writeMicroseconds(escOut);                        //write the value
    	}

        // Publish the values to the Particle Cloud
        if (currentTime - lastPublishTime >= PUBLISH_INTERVAL_MS) {
            lastPublishTime = currentTime;
            particleCurrent = filteredCurrent * (-1);
            particleVoltage = filteredVoltage;
            particleMotorSpeed = rawThrottle;
            Particle.publish("current", String(particleCurrent));
            Particle.publish("voltage", String(particleVoltage));
            Particle.publish("motorSpeed", String(particleMotorSpeed));
        }
    }
}