Controllable Assisted Robot Car with Robotic Gripper

Story

The Controllable Assisted Robot Car with Robotic Gripper is a final project for my Mechanical Engineering 461 Class at UIUC. We were given the freedom to create a project that would not only be interesting to yourself but also look into the TI F28379D Launchpad board more than we did before. I enjoy the way the electrical and coding side of robotics turns whatever I can think of into a reality. I am much more experienced with physical building than I am with robotic coding; therefore, I wanted to mix the two together in my final project. When we were first looking into the TI F28379D Launchpad board I saw all the possibilities it could have. I knew almost anything I could think of could be done with this board. I have always been interested in robotic manufacturing and vehicles that move objects to a desired spot. That is how I came up with the idea for a robotic gripper. I then wanted to use more sensors to make the robot foolproof in case I wanted someone else to try driving the robot. I then had the idea to use switches as a way to automatically stop the robot from getting too close to the walls.

Information on the PCB

The PCB was made from the ideas of our Professor, Dan Block. Thanks to him this PCB can be used for many more projects involving the TI F28379D Launchpad. This PCB incorporates more hardware that might not be relevant to this project. However, If you want more information on the PCB and the schematics of it you can get ahold of him at this email: d-block@illinois.edu.

Code Setup

When starting the code for this robot I made sure to set up the correct GPIO pins and their pull up. I also made sure to set up the ADC configurations as well as the DAC, PIE vector table, EPWM5, EPWM6, and EPWM8. I will be using the ADC to convert the analog signals from the joystick to digital. The DAC will then be used to convert that digital signal back to an analog signal. EPWM5 will be used to trigger the ADC. EPWM6 will to used to run the motors. While EPWM8 will then be used to run the RC servos. I also created an init_eQeps function which will be used to read the angle of the motors for a control system.

EQep Setup

void init_eQEPs(void) {
    // setup eQEP1 pins for input
    EALLOW;
    //Disable internal pull-up for the selected output pins for reduced power consumption
    GpioCtrlRegs.GPAPUD.bit.GPIO20 = 1; // Disable pull-up on GPIO20 (EQEP1A)
    GpioCtrlRegs.GPAPUD.bit.GPIO21 = 1; // Disable pull-up on GPIO21 (EQEP1B)
    GpioCtrlRegs.GPAQSEL2.bit.GPIO20 = 2; // Qual every 6 samples
    GpioCtrlRegs.GPAQSEL2.bit.GPIO21 = 2; // Qual every 6 samples
    EDIS;
    // This specifies which of the possible GPIO pins will be EQEP1 functional pins.
    // Comment out other unwanted lines.
    GPIO_SetupPinMux(20, GPIO_MUX_CPU1, 1);
    GPIO_SetupPinMux(21, GPIO_MUX_CPU1, 1);
    EQep1Regs.QEPCTL.bit.QPEN = 0; // make sure eqep in reset
    EQep1Regs.QDECCTL.bit.QSRC = 0; // Quadrature count mode
    EQep1Regs.QPOSCTL.all = 0x0; // Disable eQep Position Compare
    EQep1Regs.QCAPCTL.all = 0x0; // Disable eQep Capture
    EQep1Regs.QEINT.all = 0x0; // Disable all eQep interrupts
    EQep1Regs.QPOSMAX = 0xFFFFFFFF; // use full range of the 32 bit count
    EQep1Regs.QEPCTL.bit.FREE_SOFT = 2; // EQep uneffected by emulation suspend in Code Composer
    EQep1Regs.QEPCTL.bit.QPEN = 1; // Enable EQep
    EQep1Regs.QPOSCNT = 0;
    // setup QEP2 pins for input
    EALLOW;
    //Disable internal pull-up for the selected output pinsfor reduced power consumption
    GpioCtrlRegs.GPBPUD.bit.GPIO54 = 1; // Disable pull-up on GPIO54 (EQEP2A)
    GpioCtrlRegs.GPBPUD.bit.GPIO55 = 1; // Disable pull-up on GPIO55 (EQEP2B)
    GpioCtrlRegs.GPBQSEL2.bit.GPIO54 = 2; // Qual every 6 samples
    GpioCtrlRegs.GPBQSEL2.bit.GPIO55 = 2; // Qual every 6 samples
    EDIS;
    GPIO_SetupPinMux(54, GPIO_MUX_CPU1, 5); // set GPIO54 and eQep2A
    GPIO_SetupPinMux(55, GPIO_MUX_CPU1, 5); // set GPIO54 and eQep2B
    EQep2Regs.QEPCTL.bit.QPEN = 0; // make sure qep reset
    EQep2Regs.QDECCTL.bit.QSRC = 0; // Quadrature count mode
    EQep2Regs.QPOSCTL.all = 0x0; // Disable eQep Position Compare
    EQep2Regs.QCAPCTL.all = 0x0; // Disable eQep Capture
    EQep2Regs.QEINT.all = 0x0; // Disable all eQep interrupts
    EQep2Regs.QPOSMAX = 0xFFFFFFFF; // use full range of the 32 bit count.
    EQep2Regs.QEPCTL.bit.FREE_SOFT = 2; // EQep uneffected by emulation suspend
    EQep2Regs.QEPCTL.bit.QPEN = 1; // Enable EQep
    EQep2Regs.QPOSCNT = 0;
}

ADC and DAC Setup

EALLOW;

    //write configurations for all ADCs  ADCA, ADCB, ADCC, ADCD
    AdcaRegs.ADCCTL2.bit.PRESCALE = 6; //set ADCCLK divider to /4
    AdcbRegs.ADCCTL2.bit.PRESCALE = 6; //set ADCCLK divider to /4
    AdccRegs.ADCCTL2.bit.PRESCALE = 6; //set ADCCLK divider to /4
    AdcdRegs.ADCCTL2.bit.PRESCALE = 6; //set ADCCLK divider to /4
    AdcSetMode(ADC_ADCA, ADC_RESOLUTION_12BIT, ADC_SIGNALMODE_SINGLE);  //read calibration settings
    AdcSetMode(ADC_ADCB, ADC_RESOLUTION_12BIT, ADC_SIGNALMODE_SINGLE);  //read calibration settings
    AdcSetMode(ADC_ADCC, ADC_RESOLUTION_12BIT, ADC_SIGNALMODE_SINGLE);  //read calibration settings
    AdcSetMode(ADC_ADCD, ADC_RESOLUTION_12BIT, ADC_SIGNALMODE_SINGLE);  //read calibration settings
    //Set pulse positions to late
    AdcaRegs.ADCCTL1.bit.INTPULSEPOS = 1;
    AdcbRegs.ADCCTL1.bit.INTPULSEPOS = 1;
    AdccRegs.ADCCTL1.bit.INTPULSEPOS = 1;
    AdcdRegs.ADCCTL1.bit.INTPULSEPOS = 1;
    //power up the ADCs
    AdcaRegs.ADCCTL1.bit.ADCPWDNZ = 1;
    AdcbRegs.ADCCTL1.bit.ADCPWDNZ = 1;
    AdccRegs.ADCCTL1.bit.ADCPWDNZ = 1;
    AdcdRegs.ADCCTL1.bit.ADCPWDNZ = 1;
    //delay for 1ms to allow ADC time to power up
    DELAY_US(1000);

    //Select the channels to convert and end of conversion flag
    //Many statements commented out,  To be used when using ADCA or ADCB
    //ADCA
    AdcaRegs.ADCSOC0CTL.bit.CHSEL = 0x02;  //SOC0 will convert Channel you choose Does not have to be A0
    AdcaRegs.ADCSOC0CTL.bit.ACQPS = 14; //sample window is acqps + 1 SYSCLK cycles = 75ns
    AdcaRegs.ADCSOC0CTL.bit.TRIGSEL = 0x0D;// EPWM5 ADCSOCA or another trigger you choose will trigger SOC0
    AdcaRegs.ADCSOC1CTL.bit.CHSEL = 0x03;  //SOC1 will convert Channel you choose Does not have to be A1
    AdcaRegs.ADCSOC1CTL.bit.ACQPS = 14; //sample window is acqps + 1 SYSCLK cycles = 75ns
    AdcaRegs.ADCSOC1CTL.bit.TRIGSEL = 0x0D;// EPWM5 ADCSOCA or another trigger you choose will trigger SOC1
    AdcaRegs.ADCINTSEL1N2.bit.INT1SEL = 1; //set to last SOC that is converted and it will set INT1 flag ADCA1
    AdcaRegs.ADCINTSEL1N2.bit.INT1E = 1;   //enable INT1 flag
    AdcaRegs.ADCINTFLGCLR.bit.ADCINT1 = 1; //make sure INT1 flag is cleared
    EDIS;
    // Enable DACA and DACB outputs
    EALLOW;
    DacaRegs.DACOUTEN.bit.DACOUTEN = 1; //enable dacA output-->uses ADCINA0
    DacaRegs.DACCTL.bit.LOADMODE = 0;   //load on next sysclk
    DacaRegs.DACCTL.bit.DACREFSEL = 1;  //use ADC VREF as reference voltage

    DacbRegs.DACOUTEN.bit.DACOUTEN = 1; //enable dacB output-->uses ADCINA1
    DacbRegs.DACCTL.bit.LOADMODE = 0;   //load on next sysclk
    DacbRegs.DACCTL.bit.DACREFSEL = 1;  //use ADC VREF as reference voltage
    EDIS;

Motor PI Controller

The motors will be controlled by a coupled PI controller shown above. This controller will filter out any error signal caused by the motor, as well as allow me to also filter error when turning the robot. The reference velocity will be the forward and backward directions on the joystick, while the reference velocity for each individual motor will increase and decrease with the left and right on the joystick for turning. This final velocity for both the left and the right motor will be sent to the E6PWMA's or B's CMPA to run the motors. The integral below will help reduce the windup when starting the motor up each time.

PI Controller

RightWheel = readEncRight();
    LeftWheel = readEncLeft();

    // distance ft
    XLeftK = 1/(radftL/LeftWheel);
    XRightK = 1/(radftR/RightWheel);
    // vel ft/s
    VLeftK = (XLeftK - XLeftK1)/0.004;
    VRightK = (XRightK - XRightK1)/0.004;

    // PI Controller

    eturn = turn + (VLeftK - VRightK);

    ekLeft = Vref - VLeftK - (Kturn*eturn);
    IkLeft = IkLeft1 + 0.004*((ekLeft + ekLeft1)/2);
    uLeft = (Kp*ekLeft) + (Ki*IkLeft);

    ekRight = Vref - VRightK + (Kturn*eturn);
    IkRight = IkRight1 + 0.004*((ekRight + ekRight1)/2);
    uRight = (Kp+ekRight) + (Ki*IkRight);

    //integral
    if(fabs(uLeft) >= 20) {
        IkLeft = IkLeft1;
    }
    if(fabs(uRight) >= 20){
        IkRight = IkRight1;
    }

    setEPWM6A(uLeft);
    setEPWM6B(-uRight);

    //states
    XLeftK1 = XLeftK;
    XRightK1 = XRightK;
    IkLeft1 = IkLeft;
    IkRight1 = IkRight;
    ekLeft1 = ekLeft;
    ekRight1 = ekRight;

Joystick Setup

The Joystick is read using the ADC which tends to have electrical noise. This is counteracted using a Discrete Filter. The filter I used came from MatLab by using a 9th order FIR filter with a 50Hz cutoff frequency. The resulting filtered values are then sent to the DAC which is then used as the reference velocity for the motors. The joystick can then run both motors.

Discrete Filter

GpioDataRegs.GPBSET.bit.GPIO52 = 1;
        UnScVoltage0 = AdcaResultRegs.ADCRESULT0;
        UnScVoltage1 = AdcaResultRegs.ADCRESULT1;
        // Here covert ADCIND0, ADCIND1 to volts
        dacouta0 = (UnScVoltage0/4095.0)*3 ;
        dacouta1 = (UnScVoltage1/4095.0)*3 ;
        xk0array[0] = dacouta0;
        xk1array[0] = dacouta1;
        yk = 0;
        yk1 = 0;
        int j = 0;
        for ( j=0; j < FILTER_SIZE ; j++){  //solving for filtered value of dacout0
            yk += b[j]*xk0array[j];
        }
        for ( j=0; j < FILTER_SIZE ; j++){  //solving for filtered value of dacout1
            yk1 += b[j]*xk1array[j];
        }
//        yk = b[0]*xk_0 + b[1]*xk_1 + b[2]*xk_2 + b[3]*xk_3 + b[4]*xk_4;

        //Save past states before exiting from the function so that next sample they are the older state
//        xk_4 = xk_3;
//        xk_3 = xk_2;
//        xk_2 = xk_1;
//        xk_1 = xk_0;
        setDACA(yk+1.5);    // setting filtered value to the DACA with off set of 1.5
        setDACB(yk1+1.5);
        int i = 0 ;
        for (i = FILTER_SIZE-1; i > 0 ; i--){
            xk0array[i] = xk0array[i-1];
        }
        for (i = FILTER_SIZE-1; i > 0 ; i--){
                   xk1array[i] = xk1array[i-1];
               }

RC Servo Setup

Next, the RC servos are ran using EPWM8. To get a certain angle on the servo I had to set EPWM8' CMPA value to a certain duty cycle. After experimenting with the angle I need, I used the duty cycles I had to have an open and closed state for the gripper servo and an up and down state for the robotic arm servo. Using a clever way of counting with a modulus operation I was able to set the servos to a four-stage cycle that would repeat every four increments. Three LED lights are shown as the robot goes through each stage to show the operator which stage the robot is at. These increments will be controlled by a switch controlled by the operator.

Servo States

//Set EPWM8A for RC servo 1, up and down
void setEPWM8A(uint16_t angle){
    // stage 1 = down/restart
    if ((angle % 4) == 0){
        duty = EPwm8Regs.TBPRD * 0.065;
        EPwm8Regs.CMPA.bit.CMPA = duty;
        GpioDataRegs.GPDCLEAR.bit.GPIO111 = 1; // LED7 Off
        GpioDataRegs.GPECLEAR.bit.GPIO130 = 1; // LED8 Off
        GpioDataRegs.GPECLEAR.bit.GPIO131 = 1; // LED9 Off
    }
    // stage 3 = up
    if ((angle % 4) == 2){
        duty = EPwm8Regs.TBPRD * 0.09;
        EPwm8Regs.CMPA.bit.CMPA = duty;
        GpioDataRegs.GPESET.bit.GPIO130 = 1; // LED8 On
    }
    // stage 4 = down
    if ((angle % 4) == 3){
            duty = EPwm8Regs.TBPRD * 0.065;
            EPwm8Regs.CMPA.bit.CMPA = duty;
            GpioDataRegs.GPESET.bit.GPIO131 = 1; // LED9 On
        }
}
//Set EPWM8B for RC servo 2, gripper
void setEPWM8B(uint16_t angle){
    // stage 1 = open/restart
    if ((angle % 4) == 0){
        duty = EPwm8Regs.TBPRD * 0.03;
        EPwm8Regs.CMPB.bit.CMPB = duty;
    }
    // stage 2 = close
    if ((angle % 4) == 1){
            duty = EPwm8Regs.TBPRD * 0.08;
            EPwm8Regs.CMPB.bit.CMPB = duty;
            GpioDataRegs.GPDSET.bit.GPIO111 = 1; // LED7 On
        }
}

Switch Setup

The three switches used are set to always give current and break the circuit when pressed. These are set to GPIO pins and soldered to the PCB. One of the switches will control what stage the robotic gripper system is in. This will be done by having an interrupt function check for the value of the corresponding GPIO pin every half a second and if it sees that the valve has changed it will increment the stage variable up one. The other two switches will be used to assist the operator if they come too close to a wall. The switches will be strapped to the front of the robot with a piece of flexible music wire pointing outward. When the music wire comes into contact with the wall the wire will bend and start to put pressure on the switch until it is pressed. There will be an interrupt function checking the value of the switch every millisecond and will set the turn variable to turn the robot away from the wall if the value of the corresponding GPIO pin value is changed.

Building the Robotic Arm

The last design will have to be the self-built robotic arm. The initial prototype of the robotic arm is shown above. It will use two servos. The weaker one will open the robotic gripper by pulling two strings back and the other one will raise and lower the gripper. The initial design puts the strings attached to the furthest point on the gripper arm; however, the final design needed to add a bar to each side to extend the furthest distance. This will allow the servo to have more leverage to pull back the gripper arms. The arms stay together because of a rubber band in the middle of them. The stages of the robot system start with the initial stage 0 having the gripper open and the servo 2 in the down state. Stage 2 closes the gripper. Stage 3 lifts up the gripper using servo 2 in the up state. Lastly, stage 4 brings the arm back down by putting servo 2 in the down state. The whole system is bolted to the robot at the front right.

Tech Demo

Above is a video showing all of the features of the project. First, you can see the gamepad that was made for the robot it has the joystick and stage switch attached. Then you can see the robotic gripper system go through its 4 stages. Pay attention to the right of the robot and you will see 3 LEDs light up as the stages progress. Lastly, I show off the assisted driving by manually turning the car by bending the music wire on the sides.

Completed Design Analysis

Above is a video of the completed design moving a block around a track. In this video, you can see that the robot is very easy and responsive to every minor input making it simple to maneuver. You can see that the car gets too close to the walls twice in the video and is turned away automatically using the assisted steering.

Sensor and Actuator Connections

Below I show how and where the sensors and actuators are and how they are connected to the PCB and the TI F28379D Launchpad board. The two servos can be plugged into pins that are soldered to the PCB. Above, most of the red wires are the connections of the PCB to the TI F28379D Launchpad board, which is also plugged into pins soldered to the PCB. The motors are bolted to the PCB and are directly connected to the Launchpad and PCB. The motors are attached to the QEP A and B connections of the Launchpad. The switches and joystick are attached to the PCB via red and white cables. The red cables run to parts of the PCB already connected to the GPIO pins of the Launchpad. Below you can see a close up of the gripper system and how the string is attached to the servo. Overall the TI F28379D Launchpad has made this project possible. I have been able to combine my physical building knowledge and my coding knowledge to bring something from my imagination to life.