OmniDrive General Solution

omni.java

    	int wheels = 3;
    	double spinScale = 0.5;
    	double angles[] = {0, 1.507, 3.14};
    	double maxOutput = 1.0;
    	
    	// left, center, right
    	double outputs[] = {0, 0, 0};
    	
    	double maxValue = 0.0;
    	// Compute the array of inputs
    	for(int i = 0; i < wheels; i++){
    		outputs[i] = -(x)*Math.sin(angles[i])+y*Math.cos(angles[i])+(-spinScale*rot);
    		if(Math.abs(outputs[i]) > maxValue) {
    			maxValue = Math.abs(outputs[i]);
    		}
    	}
    	// If we've given any output a value greater than max
    	// then scale it down.
    	if(maxValue > maxOutput) {
    		double scale = maxOutput/maxValue;
    		for(int i = 0; i < wheels; i++) {
    			outputs[i] = outputs[i] * scale;
    		}
    	}