Wearable Control for Robot Car and Gripper

This project was made as a final project for Me461 at the University of Illinois at Urbana-Champaign, under the guidance of Professor Dan Block and using components generously given to the students by TI. I wanted to take a step towards intuitive control of the robot car and so I thought it would be cool to be able to control the motion of the car by tilting the hand in the corresponding direction. I also thought it would be nice to add a gripper to the car to make it more functional. Currently, the controls are still connected by about half a foot of wires to the car but in the future I'd like to work towards communicating this data wirelessly to the car. All of the code is written in C.

Before you start

At the core of this project is the F28379D Launchpad, which is connected to a PCB designed by Dan Block. For a copy of the board files or information on what specific parts were used to bring it all together (such as h-bridges, power supplies, etc.) you can email him at d-block@illinois.edu. A lot of the base code was also provided and won't be explained in-depth. In general, a working robot car controlled with the F28379D is needed, along with the MPU-9250, a flex sensor, a way to attach the servo-controlled gripper, and lastly the servo and gripper.

Physical Assembly and Wiring

Attaching the Gripper and Servo

I started by attaching the gripper to the front of the car. I used a portion of an old robot's body to help secure it to the PCB using some of the existing screw holes and using rubber bands to attach the actual gripper (just because I didn't have any extra screws at the time). The gripper was assembled with the servo motor as explained in their own instructions. The servo used in this project has 3 pins: Vcc, Gnd, and the Pwm control pin. I connected the Pwm pin to pin 74 on the F28379D.

On the underside of the servo is another portion of my old robot, being used as a sort of front wheel since the white and silver roller ball wasn't high enough to keep the servo from hitting the ground. This is not the most stable way to assemble these parts and I would instead recommend designing the PCB with some extra screw holes and/or 3D printing parts to connect the gripper to the PCB more precisely.

Attaching the MPU-9250 and Flex Sensor

The MPU-9250 returns gyroscope and accelerometer readings. The accelerometer readings will be used to detect what direction the sensor is being tilted to and will be used to command the motors so the robot car goes in the corresponding direction. A flex sensor is a sensor that increases resistance when it is bent, and the ADC inputs on the F28379D will be used to detect if the flex sensor is bent and if it should actuate the gripper. Since the eventual plan is to make these controls wireless, the wiring connecting the MPU-9250 and flex sensor is fairly temporary. Some female/male extension jumper wires were used to connect Vcc, Gnd, SCL, MOSI/SDA, MISO/AD0, and NCS on the PCB/MPU-9250. The other pins aren't needed for this project. As far as wiring to the F28379D:

• SCL is connected to pin 47
• MOSI/SDA is connected to pin 55
• MISO/AD0 is connected to pin 54
• and NCS is connected to pin 59

The flex sensor is wired according to the image below, where the yellow wire then connects to pin 26 on the F28379D, which is configured as ADCINA3. I chose this pin because it was convenient as a result of previous projects and the code I had already written, but any ADCIN pin will do as long as it is coded correspondingly.

My solution to quickly make this wearable was to fold up a notecard and cut a slit for the MPU-9250's pins. I also taped the flex sensor to the inside of the notecard and then taped the notecard's open ends so that it is a stiffer whole piece. This kept the MPU-9250 from rotating in my palm because the notecard made it a wider, flatter object and it helped keep the flex sensor at a length to my finger so that I would bend the middle of it when my fingers curled. To attach this to my hand, I just slipped a hair-tie around the notecard and slid my it around my palm.

Programming

All of the code that will be gone over is in the project_main.c code attached. Code snippits won't include everything but will try to include important highlights.

Reading the Flex Sensor

First, the ADCA interrupt service routine needs to be made and configured to sample the resistance of the flex sensor going into pin 26. I configure this in my setupADC() function, which starts on line 718 and is called on line 279 in the main() function. The necessary parts are shown below:

void setupADC(void) {
    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
    AdcaRegs.ADCSOC0CTL.bit.CHSEL = 0x3; //SOC0 will convert Channel ADCINA2
    AdcaRegs.ADCSOC0CTL.bit.ACQPS = 14; //sample window is acqps + 1 SYSCLK cycles = 75ns
    AdcaRegs.ADCSOC0CTL.bit.TRIGSEL = 0xD;// EPWM5 ADCSOCA
    AdcaRegs.ADCINTSEL1N2.bit.INT1SEL = 0; //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;
}

To finish setting up the ADC to read information, the PieVectTable (line 270) and PieCtrlRegs (line 317) need to be updated in the main() function as well:

PieVectTable.ADCA1_INT = &ADCA_ISR;     // tells processor to call ADCA_ISR when ADCA2 interrupt occurs
// Enables PIE interrupt 1.1
    PieCtrlRegs.PIEIER1.bit.INTx1 = 1;      // ADCA1, Table3-2 in ADC technical reference

Next, in the ADCA_ISR() function (line 423), the resistance of the flex sensor is read, converted to volts, and stored as a global variable. This can be used directly but I manipulated it more later.

__interrupt void ADCA_ISR(void) {
    GpioDataRegs.GPBSET.bit.GPIO52 = 1;

    // FLEX SENSOR READING
    float adca3out = AdcaResultRegs.ADCRESULT0;       // ADCINA3 is connected to pin 26 = flex sensor
    adcina3Volts = adca3out * (3.0/4095.0);

    // CLEAR INTERRUPT FLAG
    AdcaRegs.ADCINTFLGCLR.bit.ADCINT1 = 1;
    PieCtrlRegs.PIEACK.all = PIEACK_GROUP1;
    GpioDataRegs.GPBCLEAR.bit.GPIO52 = 1;
}

Reading the MPU-6250

The MPU-6250 uses SPI to communicate. Setting up the SPI to begin to communicate with the MPU-6250 begins on line 801 and goes until line 978, This was completed with extensive help from Professor Dan Block and will not be gone into in depth. There are several lines in the main() function that need to be included such as:

PieVectTable.SPIB_RX_INT = &SPIB_isr;

on line 273,

IER |= M_INT6;      // SPIB_RX

on line 310, and

// Enable SPIB_RX interrupt: 6.3
    PieCtrlRegs.PIEIER6.bit.INTx3 = 1;

on line 318. To read values from the MPU-6250, an equal number of values need to be transmitted to it. This can be added to the ADCA_ISR() function as shown below:

__interrupt void ADCA_ISR(void) {
    GpioDataRegs.GPBSET.bit.GPIO52 = 1;

    // FLEX SENSOR READING
    float adca3out = AdcaResultRegs.ADCRESULT0;       // ADCINA3 is connected to pin 26 = flex sensor
    adcina3Volts = adca3out * (3.0/4095.0);

    // IMU READINGS
    GpioDataRegs.GPCCLEAR.bit.GPIO66 = 1;       // GPIO66 low to act as slave select
    SpibRegs.SPIFFRX.bit.RXFFIL = 8;        // issue the SPIB_RX_INT when all values are in RX FIFO
    // get accel readings
    SpibRegs.SPITXBUF =  ((0x8000) | (0x3A00));      // read = 0x8000, register = 0x3A00 = INT_STATUS start
    SpibRegs.SPITXBUF = 0;      // full 16 bits of ACCEL_XOUT
    SpibRegs.SPITXBUF = 0;      // full 16 bits of ACCEL_YOUT
    SpibRegs.SPITXBUF = 0;      // full 16 bits of ACCEL_ZOUT
    SpibRegs.SPITXBUF = 0;      // temp
    // get gyro readings
    SpibRegs.SPITXBUF = 0;      // full 16 bits of GYRO_XOUT
    SpibRegs.SPITXBUF = 0;      // full 16 bits of GYRO_YOUT
    SpibRegs.SPITXBUF = 0;      // full 16 bits of GYRO_ZOUT

    // CLEAR INTERRUPT FLAG
    AdcaRegs.ADCINTFLGCLR.bit.ADCINT1 = 1;
    PieCtrlRegs.PIEACK.all = PIEACK_GROUP1;
    GpioDataRegs.GPBCLEAR.bit.GPIO52 = 1;
}

Even though the gyroscope isn't being used, it will still be included. Reading the values from the MPU-6250 is done in the SPIB_isr() function as shown below:

__interrupt void SPIB_isr(void) {
    SPIB_isr_count++;

    // RECIEVE DATA
    GpioDataRegs.GPCSET.bit.GPIO66 = 1;     // deselect
    // read values
    int16_t temp = SpibRegs.SPIRXBUF;      // reads start for gyro
    int16_t accelXraw = SpibRegs.SPIRXBUF;      // reads full accel x
    int16_t accelYraw = SpibRegs.SPIRXBUF;      // reads full accel y
    int16_t accelZraw = SpibRegs.SPIRXBUF;      // reads full accel z
    temp = SpibRegs.SPIRXBUF;      // reads temp
    int16_t gyroXraw = SpibRegs.SPIRXBUF;       // reads full gyro x
    int16_t gyroYraw = SpibRegs.SPIRXBUF;       // reads full gyro y
    int16_t gyroZraw = SpibRegs.SPIRXBUF;       // reads full gyro z
    // manipulate values
    accelXreading = accelXraw*4.0/32767.0;      // scale to g (-4g to 4g)
    accelYreading = accelYraw*4.0/32767.0;
    accelZreading = accelZraw*4.0/32767.0;
    gyroXreading = gyroXraw*250.0/32767.0;      // scale to degrees/second (-250 to 250 degrees per sec)
    gyroYreading = gyroYraw*250.0/32767.0;
    gyroZreading = gyroZraw*250.0/32767.0;

    // SENSE MOTOR ANGLES
    leftWheel = -readEncLeft();     // disance in radians
    rightWheel = -readEncRight();        // negative is to compensate for the weird wiring mixup

    // EXITING INTERRUPT
    if ((SPIB_isr_count % 200) == 0) UARTPrint = 1;     // printing to UART every 100ms
    SpibRegs.SPIFFRX.bit.RXFFOVFCLR = 1; // Clear Overflow flag just in case of an overflow
    SpibRegs.SPIFFRX.bit.RXFFINTCLR = 1; // Clear RX FIFO Interrupt flag so next interrupt will happen
    PieCtrlRegs.PIEACK.all = PIEACK_GROUP6; // Acknowledge INT6 PIE interrupt
}

You can see that the readings are scaled and that after reading from the MPU-6250, the motor angles are read. Some of the signs in my code are confusing because one of the motor encoders I got had its wires flipped.

Calibrating and Filtering the MPU-6250 and Flex Sensor

The robot car is programmed to spend the first few seconds after turning on calibrating the MPU-6250 and the flex sensor. This means that during the first few seconds (the blue LED on the F28379D will blink while calibrating) it is important to hold your hand as still as possible and in the neutral position you want it to be in where the car should not be moving and the servo motor should not be actuated. Again, a lot of the calibration code was provided by Professor Dan Block and so it will not be explained in depth here. It ranges from lines 478-558. After calibrating, the values are all put through a five-point averaging filter. This helps to reduce some noise and all the further calculations are done using the filtered values.

Controlling the Servo Motor with the Flex Sensor

The servo motor I used is a continuous rotation servo motor, so the Pwm input controls its speed as opposed to its position. I set it up to be controlled by EPWM8A, as shown below:

void setupEPWM8(void) {
    EALLOW;
    EPwm8Regs.TBCTL.bit.CTRMODE = 0x0;     // Up-count mode
    EPwm8Regs.TBCTL.bit.FREE_SOFT = 0x2;      // Free run; 1x means use any value that isn't already in use
    EPwm8Regs.TBCTL.bit.PHSEN = 0x0;       // disable phase loading
    EPwm8Regs.TBCTL.bit.CLKDIV = 0x4;      // clock divide to 16, 50,000,000/16 = 3,125,000
    EPwm8Regs.TBCTR = 0;     // start timers at zero
    //EPwm8Regs.TBPRD = 2500;     // (1/20,000)/(1/50,000,000) = 2500 b/c main clock ctr is 50MHz and prd here is 20KHz
    EPwm8Regs.TBPRD = 62500;    // (1/50)/(1/3,125,000) = 0.02 b/c main clock ctr is 50MHz and prd here is 50Hz = 0.02sec
    EPwm8Regs.CMPA.bit.CMPA = .13*EPwm8Regs.TBPRD;    // for continuous rotation servo, start duty cycle at ~ 1.5ms = stopped
    EPwm8Regs.CMPB.bit.CMPB = .08*EPwm8Regs.TBPRD;      // for position servo, start duty cycle at 8% = 0 degrees
    EPwm8Regs.AQCTLA.bit.CAU = 0x1;    // when TBCTR = CMPB on up count, clear (force EPWMxA output low)
    EPwm8Regs.AQCTLB.bit.CBU = 0x1;
    EPwm8Regs.AQCTLA.bit.ZRO = 0x2;    // when TBCTR = 0, set high
    EPwm8Regs.AQCTLB.bit.ZRO = 0x2;
    EPwm8Regs.TBPHS.bit.TBPHS = 0;     // set phase to zero
    GPIO_SetupPinMux(14, GPIO_MUX_CPU1, 1);        // set GPIO14 to EPWM8A: RCServo1
    GPIO_SetupPinMux(15, GPIO_MUX_CPU1, 1);     // set GPIO15 to EPWM8B: RCServo2
    EDIS;
}

After some testing, I found that setting EPwm8Regs.CMPA.bit.CMPA = 0.13*EPwm8Regs.TBPRD stops the motor from moving. I set the close speed to be a multiplier of 0.07 and the open speed as a multiplier of 0.08 because those are the slowest speeds going in either direction. I used the code shown below to set the servo to actuate while the flex sensor is bent and to toggle the direction it goes between each bend. There is an issue where sometimes it misses a toggle that I haven't quite figured out yet. The code shown below is in SPIB_isr() function, just after the calibration code.

// gripper control
    if ((flex_filtered > 0.1) && (servoCtrl > 0.12)) {      // flex sensor bent and servo is not moving
        if (closing) servoCtrl = closeSpeed;
        else servoCtrl = openSpeed;
    }
    if ((flex_filtered <= 0.1) && (servoCtrl <= 0.12)) {     // flex sensor not bent and servo is moving
        servoCtrl = 0.13;
    }
    // this is a little buggy, probably because it takes a moment for the values to update
    if ((servoCtrl > 0.12) && (servoCtrl_prev <= 0.12)) {
        closing = !closing;     // motor just stopped moving, toggle direction
    }
    servoCtrl_prev = servoCtrl;

    EPwm8Regs.CMPA.bit.CMPA = servoCtrl*EPwm8Regs.TBPRD;

Controlling Car Movement with the MPU-6250

EPWM6A and EPWM6B are used to control the left and right motors, respectively. The Pwm pins are set up in the main() function as shown below:

// setup EPWM6A and B with 20kHz carrier frequency
    EALLOW;
    EPwm6Regs.TBCTL.bit.CTRMODE = 0x0;     // Up-count mode
    EPwm6Regs.TBCTL.bit.FREE_SOFT = 0x2;      // Free run; 1x means use any value that isn't already in use
    EPwm6Regs.TBCTL.bit.PHSEN = 0x0;       // disable phase loading
    EPwm6Regs.TBCTL.bit.CLKDIV = 0x0;      // clock divide to 1
    EPwm6Regs.TBCTR = 0;     // start timers at zero
    EPwm6Regs.TBPRD = 2500;     // (1/20,000)/(1/50,000,000) = 2500 b/c main clock ctr is 50MHz and prd here is 20KHz
    EPwm6Regs.CMPA.bit.CMPA = 0;    // start duty cycle at 0%
    EPwm6Regs.CMPB.bit.CMPB = 0;
    EPwm6Regs.AQCTLA.bit.CAU = 0x1;    // when TBCTR = CMPB on up count, clear (force EPWMxA output low)
    EPwm6Regs.AQCTLB.bit.CBU = 0x1;
    EPwm6Regs.AQCTLA.bit.ZRO = 0x2;    // when TBCTR = 0, set high
    EPwm6Regs.AQCTLB.bit.ZRO = 0x2;
    EPwm6Regs.TBPHS.bit.TBPHS = 0;     // set phase to zero
    EDIS;
    GPIO_SetupPinMux(10, GPIO_MUX_CPU1, 1);     // set DRV1PWM from GPIO10 to EPWM6A
    GPIO_SetupPinMux(11, GPIO_MUX_CPU1, 1);     // set DRV2PWM from GPIO11 to EPWM6B

The functions setEPWM6A() and setEPWM6B() are used to saturate the control effort to each wheel to be between -10 and 10, to determine the direction the motors should turn, and then to set the duty cycle to the appropriate percent. The x accelerometer reading from the MPU-6250 is used to determine if the car is moving forward or backwards and at what speed (tilting more goes faster). The y accelerometer reading corresponds to which direction to turn. The code below shows how these readings are manipulated to turn into control efforts for each wheel. They are both scaled down significantly to keep the speeds in a reasonable range.

// wheel control
    fwd_bwd += (accelx_filtered/10000);      // scales accelx_filtered to make wheels go slower
    turn += (accely_filtered/15000);
    uLeft = fwd_bwd + turn;
    uRight = fwd_bwd - turn;

    setEPWM6A(uLeft);
    setEPWM6B(uRight);

Results

Putting all of that together creates a robot car that is controlled by the direction you tilt your hand and that has a gripper controlled by your grip. The next step to keep improving this project would be to start working towards making it wireless control.