예제 #1
0
void goAhead(int grids) {
    MODE = 1;
    delta = 0;
    currentSpeed = speedModeSpeed;
    if (grids == 1) {
        rightMCtr = leftMCtr = RisingEdgePerGrid_300;
    } else {
        MODE = 2;
        currentSpeed = fastRunSpeed;
        rightMCtr = leftMCtr = RisingEdgeForSP * grids;
    }

    setTimerInterrupt();
    attachInterrupt(1, countRight, RISING);

    md.init();
    // md.setM2Speed(currentSpeed);
    // delay(4);
    // md.setM1Speed(currentSpeed);
    md.setSpeeds(currentSpeed, currentSpeed);
    while (--leftMCtr) {
        while (digitalRead(motor_L));
        while (!digitalRead(motor_L));
        //wait for one cycle
    }

    detachTimerInterrupt();
    detachInterrupt(1);

    brakeForGoAhead();
    
    //update direciton
    switch (pwd) {
        case 1: currentY -=grids;

                if (currentY < 2) {
                    currentY = 2;
                }
                break;
        case 2: currentX +=grids;
                if (currentX > 19) {
                    currentX = 19;
                }
                break;
        case 3: currentY +=grids;
                if (currentY > 14) {
                    currentY = 14;
                }
                break;
        case 4: currentX -=grids;
                if (currentX < 2) {
                    currentX = 2;
                }
                break;
        default: break;
    }
    //if (MODE != 2) {
        delay(50);
    //}
}
void loop() {
    int grid = 3;
    delta = 0;
    timer = 0;
    rightMCtr = leftMCtr = 400 * grid;

    setTimerInterrupt();
    attachInterrupt(1, countRight, RISING);

    md.init();
    md.setSpeeds(300, 300);
    while (--leftMCtr) {
        while (digitalRead(motor_L));
        while (!digitalRead(motor_L));
        //wait for one cycle
    }

    detachTimerInterrupt();
    detachInterrupt(1);

    md.brakeWithABS();
    Serial.println(delta);
    Serial.println(timer);
    delay(3000);
}
예제 #3
0
void setup() {

    Serial.begin(9600);
    Serial.println("CLEAR");
    setPinsMode();

    //set up motor
    md.init();

    //set up 3 Ultrasonic
    u_F.init(urPWM_F, urTRIG);
    u_L.init(urPWM_L, urTRIG);
    u_R.init(urPWM_R, urTRIG);

    //set up IR sensor
    shortIR_RF.init(shortIR_RF_in);
    shortIR_LF.init(shortIR_LF_in);

    shortIR_R.init(shortIR_R_in);
    shortIR_L.init(shortIR_L_in);
    longIR_L.init(longIR_L_in);
}
예제 #4
0
void turn(int turnRight) {
    MODE = 0;
    direction = turnRight;
    delta = 0;
    if (turnRight == 1) {
        rightMCtr = leftMCtr = RisingEdgePerTurnRight_200;
    } else {
        rightMCtr = leftMCtr = RisingEdgePerTurnLeft_200;
    }

    setTimerInterrupt();
    attachInterrupt(1, countRight, RISING);

    //sp.setSpeedLvls(3, 3);
    md.init();
    md.setSpeeds(-200 * direction, 200 * direction);
    while (--leftMCtr) {
        while (digitalRead(motor_L));
        while (!digitalRead(motor_L));
        //wait for one cycle
    }

    detachTimerInterrupt();
    detachInterrupt(1);

    brakeForRotation();
    
    //update direction
    pwd += turnRight;
    if (pwd == 5) {
        pwd = 1;
    } else if (pwd == 0) {
        pwd = 4;
    }
    delay(30);
}
예제 #5
0
int main() {


    // MPU6050 object "MPU6050.h"
    MPU6050 accelgyro;
    // Kalman filter Object "Kalman.h"
    Kalman kalman;

    // Xbee serial baudrate
    xBee.baud(38400);
    pc.baud(9600);

    // Initialize timers
    Timer timer, code, encTimer;
    // Start timers
    timer.start(); 
    code.start();
    encTimer.start();
    // Reset timer values
    timer.reset();
    code.reset();
    encTimer.reset();

    //Encoder 1 interrupts
    enc1a.rise(&incrementEnc1a);
    enc1b.rise(&incrementEnc1b);
    enc1a.fall(&setEnc1aFall);
    enc1b.fall(&setEnc1bFall);

    enc2a.rise(&incrementEnc2a);
    enc2b.rise(&incrementEnc2b);
    enc2a.fall(&setEnc2aFall);
    enc2b.fall(&setEnc2bFall);

    // Debug leds
    myled1 = 0;
    myled2 = 0;
    myled3 = 0;

    // Encoder 1 initializations
    newTime =0; oldTime = 0; lastEncTime = 0;
    enc1Speed = 0; enc2Speed = 0;

    // MPU6050 initializations
    accelgyro.initialize();
    accelgyro.setFullScaleGyroRange(0);
    accelgyro.setFullScaleAccelRange(0);

    // Initialize MotorShield object
    shield.init();

    float measuredTheta , measuredRate, newTheta,calcRate;
    float position= 0, velocity = 0, lastPosition = 0, lastVelocity =0;
    float angleOffset = 0, positionOffset = 0,lastAngleoffset = 0;


    // Position control constants, if necessary
    float zoneA = 16000.0f, zoneB = 8000.0f, zoneC = 2000.0f;
    float multiplier = 1.0f;
    float zoneAscale = 600*2*multiplier, 
        zoneBscale = 800*2*multiplier, 
        zoneCscale = 1000*2*multiplier,
        zoneDscale =  1500*2*multiplier,
        velocityScale = 60*2*multiplier;

    // Serial communication buffer
    char buffer[40];
    int i, strSize;

    // Control parameters
    float k0,k1,k2,k3,tref;
    float x, dotx, deltaX, deltaT, lastX = 0;

    // Helper variables
    float waittime , u, diff, dt;

    float error = 0,lastError = 0;
    int counter = 0;
    float pTerm = 0,dTerm = 0,iTerm = 0;


    
    while(1) {

        ///////////////////////////////////////////
        // Read serial data and update constants //
        ///////////////////////////////////////////
        i = 0;
        char a;
        while(pc.readable()){
            a = pc.getc();
            buffer[i++] = a;
            printf("%c\n", a );
            myled1 = 1;
            wait(0.001);
         }

        strSize = i;
        string receive(buffer,strSize); // Changes from char to string
        if(strSize > 0){
            printf("%s\n", receive);
            assignGainsFromString(&k[0], &k[1], &k[2], &k[3], &k[4],receive);
        }

        // Below is an attempt to control the robot position, 
        // by dividing it into "zones" and changing the angle accordingly.
        // 
        /////////////////////////////
        // Generate encoder offset //
        /////////////////////////////

        // position = (float)enc2total;
        // positionOffset = position;

        // //if((encTimer.read_us() - lastEncTime) > 0.0f ) {   // every 100 ms
            
        //     float sign = 0;
        //     if(positionOffset > 0) sign= 1;
        //     else sign = -1;


        //     if(abs(positionOffset) > zoneA) angleOffset += 1.0f/zoneAscale*positionOffset;
        //     else if(abs(positionOffset) > zoneB) angleOffset += 1.0f/zoneBscale*positionOffset;
        //     else if(abs(positionOffset) > zoneC) angleOffset += 1.0f/zoneCscale*positionOffset;
        //     else angleOffset += positionOffset/zoneDscale;

        //     printf("angleOffset: %f, positionoffset: %f\n", angleOffset, positionOffset );
        //     // Estimate velocity
        //     // 
        //     velocity = (position - lastPosition);
        //     lastPosition = position;

        //     angleOffset += velocity/velocityScale;

        //     angleOffset = constrain(angleOffset,-10, 10);
            
        //     lastAngleoffset = angleOffset;
        // //}

        // angleOffset = constrain(angleOffset,lastAngleoffset - 1, lastAngleoffset + 1);
        
        timer.reset();
        ///////////////////////////
        // Get gyro/accel values //
        ///////////////////////////
        
        accelgyro.getAcceleration(&ax,&ay,&az);
        measuredRate = accelgyro.getRotationX()*1.0/131.0f;  // Units depend on config. Right now 250o/s
        measuredTheta = -atan(1.0*ay/az);   
        measuredTheta = measuredTheta*180/PI;  // measured in degrees
        
        ///////////////////
        // Kalman Filter //
        ///////////////////
        dt = (double)(code.read_us() - oldTime)/1000000.0f;
        newTheta = kalman.getAngle(measuredTheta,
            -measuredRate, dt);
        
        //DEBUG: printf("%g \n",  (double)(code.read_us() - oldTime)/1000000.0f);
        oldTime = code.read_us();

        //////////////////
        // Control loop //
        //////////////////

        // Set control variable to zero
        u = 0;
        
        // Extract constants from k vector, which has the serial readings.
        float kp = k[0];
        float ki = k[1];
        float kd = k[2];
        tref = k[3] - angleOffset;
        waittime = k[4];

        if(newTheta >= 50 || newTheta <= -50){
            u = 0;
        }else{

            // Define error term
            error = newTheta - tref;
            // Proportional term
            pTerm = kp*error;
            //Integral term
            iTerm += ki*error*dt*100.0f;
            // Prevent integration windup:
            if(iTerm >= 100) iTerm = 100; 
            else if (iTerm <= -100) iTerm = -100;

            u = -(iTerm + pTerm);
            
            // Calculated derivative based on smoothened angle.
            // There are two other sources for the angle here: kalman.getRate(); and measuredRate.
            calcRate = -(error-lastError)/dt;

            // Derivative term
            if(kd !=  0)
            dTerm = kd*calcRate/100.0f;
            lastError = error;

            u += dTerm;

            // Correct for dead zone --- Did not have successful results but I'll leave it here.
            // int deadzone = 20;
            // if(u < -deadzone) u = u + deadzone;
            // else if(u > deadzone) u = u - deadzone; 
            // else u = 0;
            
            // // Include saturation
            u = constrain(u,-400,400);
            
            // Flash LED to indicate loop
            if(counter % 50 == 0){
                myled3 = !myled3;    
                counter = 0;
            }
            counter++;
        }
        
        lastError = error; // update angle

        if(counter % 50 == 0){
            myled3 = !myled3;    
            // xBee.printf("%f,%f\n", newTheta,u);
            counter = 0;
        } 

        // Set motor speed
        shield.setM1Speed(-(int)u);
        shield.setM2Speed((int)u);

        // DEBUG over serial port. Use with MATLAB program "serialPlot.m" to plot these results.
        printf("%f,%f,%f\n", measuredTheta, newTheta , u);
        
        // Hold the timer until desired sampling time is reached.
        while(timer.read_us() < waittime*1000);

        // Turn off serial transmission LED
        myled1 = 0;

    }
}