File content as of revision 11:a3accd7bbf16:

#include "tuning.h"
#include "robot.h"
#include "global.h"

void SystemTuning(void) {
    InitializeRobot();
    RunMotor();
    RunNavigation();
    char key;
    float increment = 1;
    
    while (1) {
        
        ResetWatchDog();
        
        if (pc.readable()) {
            key = pc.getc();
            switch(key) {
                case 'e':
                    leftMotorPI.kP = leftMotorPI.kP + increment;
                    break;
                case 'd':
                    leftMotorPI.kP = leftMotorPI.kP - increment;
                    break;
                case 'r':
                    leftMotorPI.kI = leftMotorPI.kI + increment;
                    break;
                case 'f':
                    leftMotorPI.kI = leftMotorPI.kI - increment;
                    break;
                case 't':
                    xPI.kP = xPI.kP + increment;
                    break;
                case 'g':
                    xPI.kP = xPI.kP - increment;
                    break;
                case 'y':
                    xPI.kI = xPI.kI + increment;
                    break;
                case 'h':
                    xPI.kI = xPI.kI - increment;
                    break;
                case 'u':
                    heightPI.kP = heightPI.kP + increment;
                    break;
                case 'j':
                    heightPI.kP = heightPI.kP - increment;
                    break;
                case 'i':
                    heightPI.kI = heightPI.kI + increment;
                    break;
                case 'k':
                    heightPI.kI = heightPI.kI - increment;
                    break;
                case 'o':
                    increment = increment * 10;
                    break;
                case 'l':
                    increment = increment / 10;
                    break;                    
            }  
            leftMotorPI.kP = rightMotorPI.kP;
            leftMotorPI.kI = rightMotorPI.kI;
            pc.printf("Increment \t %f \r\n", increment);
            pc.printf("Motor \t KP: %f, KI: %f \r\n", leftMotorPI.kP, leftMotorPI.kI);
            pc.printf("Steering \t KP: %f, KI: %f \r\n", xPI.kP, xPI.kI);
            pc.printf("Speed  \t KP: %f, KI: %f \r\n\r\n\r\n", heightPI.kP, heightPI.kI);
        }
        
        Thread::wait(500); // Go to sleep for 500 ms
    }
}

//MotorTuning()
//Tune the proportional gains until the robot goes straight
// q  = +speed
// a = -speed
// w = +kp left
// S = -kp left
// e = +kp right
// d = -kp right
// z = stop
// x = start
// p = print gain
void MotorTuning(void) { 
        float leftset=0,rightset=0;
        RunMotor();
        
        pc.printf("q = +speed\r\n a = -speed\r\n w = +kp left\r\n s = -kp left\r\n e = +kp right\r\n d = -kp right\r\n z = stop\r\n x = start\r\n p = print gains\r\n");
        
        while(1){
        ResetWatchDog();    
            
        if (bt.readable()) {
        char key = bt.getc();
            switch(key) {
                case 'x':
                    rightset=5;
                    leftset=5;
                    break;
                case 'z':
                    rightset=0;
                    leftset=0;
                    break;
                case 'q':
                    rightset=rightset+1;
                    leftset=leftset+1;
                    break;
                case 'a':
                    rightset=rightset+1;
                    leftset=leftset+1;
                    break;
                case 'w':
                    leftMotorPI.kP = leftMotorPI.kP + 0.000001;
                    break;
                case 's':
                    leftMotorPI.kP = leftMotorPI.kP - 0.000001;
                    break;
                case 'e':
                    rightMotorPI.kP = rightMotorPI.kP + 0.000001;
                    break;
                case 'd':
                    rightMotorPI.kP = rightMotorPI.kP - 0.000001;
                    break;
                case 'p':
                    pc.printf("right gain: %f, left gain: %f\r\n",leftMotorPI.kP,rightMotorPI.kP);
                    break;
                }
        }

void SpeedTuning(void) {
    
}

void SteeringTuning(void) {

}

void TiltTuning(void) {
    
}