test.cpp

oldPower = 0.0
power = 0.0

oldVolt = 0.0
volt = 0.0
slope = 1
    
func calculateOAndP() :
    volt = readVolt()
    current = readCurrent()

    power = volt * current

    slope = (power - oldPower) / (volt - oldVolt)

    if slope > 0 : slope = 1
    else if slope == 0 : slope = 0
    else : slope = -1

    volt = volt + (volt - oldVolt) * slope

    oldPower = power
    oldVolt = volt
    
    return slope

main:
    
    loop:
        slope = calculateOAndP()
            
        if slope == 1:
            // drive +
        else if slope == -1:
            // drive -
        else: // slope = 0
            // drive stop