MEGR 3092 - Electric Drivetrains Team 13

// Define analog pins

const int VOLTAGE_PIN = A5; 

const int CURRENT_PIN = A4; 

const int THROTTLE_PIN = A3; 

// Define digital pins

const int ENA_PIN = D5; 

const int ENB_PIN = D6; 

const int DIR_PIN = D2; 

// Define voltage ratio and current calibration values

const float VOLTAGE_RATIO = 2.07;

const float CURRENT_CALIBRATION = -.13;

// Define variables to store raw values

int rawVoltage; 

int rawCurrent; 

int rawThrottle;

// Define variables to store filtered values

float filteredVoltage; 

float filteredCurrent; 

// Define the time interval for reading the analog inputs (in microseconds)

const unsigned long READ_INTERVAL = 10000;

// Define the time interval for outputting the filtered values (in milliseconds)

const unsigned long OUTPUT_INTERVAL = 10000;

// Define the filter coefficients for a first-order low-pass filter with a cutoff frequency of 5 Hz

const float FILTER_COEFFICIENT = 0.0183;

// Define the timer for outputting the filtered values

unsigned long outputTimer = 0;

// Define variables to control motor speed and motor direction

int motorSpeed = 0; 

int outputSpeed = 180;

int prevSpeed;

bool motorDirection = false; 

void setup() { 
    
    // Initialize the serial communication
    
    Serial.begin(9600); 

    pinMode(ENA_PIN, OUTPUT); 

    pinMode(ENB_PIN, OUTPUT); 

    pinMode(DIR_PIN, INPUT_PULLDOWN); 

    } 

void loop() { 

    // Read the analog inputs

    rawVoltage = analogRead(VOLTAGE_PIN);

    rawCurrent = analogRead(CURRENT_PIN);

    rawThrottle = analogRead(THROTTLE_PIN);

    // Convert the raw values to voltages and current

    float voltage = (rawVoltage - 0) * 6.0 / 4095.0 * VOLTAGE_RATIO;

    float current = (rawCurrent - 2050.0) * 6.0 / 4095.0 / CURRENT_CALIBRATION;

    // Filter the values

    filteredVoltage = FILTER_COEFFICIENT * voltage + (1 - FILTER_COEFFICIENT) * voltage;

    filteredCurrent = FILTER_COEFFICIENT * current + (1 - FILTER_COEFFICIENT) * current;

    // Check if it's time to output the filtered values

    if (millis() - outputTimer >= OUTPUT_INTERVAL) { Serial.print("Filtered voltage: "); 

    // Output the filtered values

    Particle.publish("Filtered current", String(filteredCurrent));
  
    Particle.publish("Filtered voltage", String(filteredVoltage));

    //Particle.publish("Raw Throttle", String(rawThrottle));
    
    Particle.publish("Reverse Swtich", String(digitalRead(DIR_PIN)));
    //Particle.publish("Prev Speed", String(prevSpeed));
    Particle.publish("Speed", String(outputSpeed));

    
    // Reset the output timer

    outputTimer = millis(); 

    } 

    // Read toggle switch for forward/backward

    motorDirection = digitalRead(DIR_PIN); 
    
    // Implement slew rate traction control
    
    // Define the maximum rate of increase in throttle per loop 

    #define SLEW_RATE 100 
    
    #define accel_step 1

    // Set motor direction to forward

    if (motorDirection == HIGH) { 
    
        // Map motor speed to throttle position
        
        motorSpeed = map(rawThrottle, 0, 4095, 210, 100);
        
        //
        
        outputSpeed = constrain(outputSpeed, 100, 210);
        
        
        if (outputSpeed != motorSpeed) {
            
            if (outputSpeed < motorSpeed) {
                
                outputSpeed += accel_step;
                
                delay(SLEW_RATE);
                
            }
            
            else {
                
                outputSpeed -= accel_step;
                
                delay(SLEW_RATE);
                
            }
        }
        
        // Run motors
            
        analogWrite(ENA_PIN, outputSpeed); 
        
        analogWrite(ENB_PIN, outputSpeed);
    
    }

    // Set motor direction to backwards 
    
    else { 
           
        // Map motor speed to throttle position
       
      motorSpeed = map(rawThrottle, 0, 4095, 160, 255);
      
        //
        
        outputSpeed = constrain(outputSpeed, 160, 255);
        
        
        if (outputSpeed != motorSpeed) {
            
            if (outputSpeed < motorSpeed) {
                
                outputSpeed += accel_step;
                
                delay(SLEW_RATE);
                
            }
            
            else {
                
                outputSpeed -= accel_step;
                
                delay(SLEW_RATE);
                
            }
        }
        
        // Run motors
            
        analogWrite(ENA_PIN, outputSpeed); 
        
        analogWrite(ENB_PIN, outputSpeed);
      
        
    } 
    
    unsigned long startTime = micros(); 
    
    while (micros() - startTime < READ_INTERVAL) { 
        
        // Do nothing 
        
        } 
        
    }