E-Puck Color Sensing Project

From Mech
Revision as of 01:42, 16 June 2008 by JamesYeung (talk | contribs)
Jump to navigationJump to search
The 'Steel Toe' programmable stiffness joint

Overview

The goal of this project was to have the e-Puck roll around on the floor under a projected image, take sensor readings with one of the R G or B sensors, send back the position and color data to a listening PC by RS232 protocol via bluetooth, and occasionally receive from the PC new (x,y,theta) data on the actual position of the e-Puck (between receiving this data, the robot keeps track of its position by dead reckoning; upon receiving the data, the robot resets its estimated position to the new data). The robot had to roll around to cover the whole projected image (which is unchanging), and at the end of the run, the host PC had to display the reconstructed image and plot the error from the actual image. On the host PC side, MATLAB was used as an interface to talk to the e-Puck and generate the appropriate graphs.

Circuitry For Color Sensor

Circuit diagram for attempt 1.

The photo diodes that I worked with were from Hamamatsu. These were the ones that were considered.

  • S9702 - RGB sensor (1mm^2 active area) - ~$9 (min 10)
  • S9032-2 - RGB sensor (2mm^2 active area) - ~$15 (min 5)
  • S6430-01 - Red color sensor (4mm^2 active area) - ~$11 (min 10)
  • S6429-01 - Green color sensor (4mm^2 active area) - ~$11 (min 10)
  • S6428-01 - Blue color sensor (4mm^2 active area) - ~$11 (min 10)

I mainly worked with the S9702 and the S6400 series sensors.

Attempt 1

I started by simply hooking the sensors with the circuit to the right. It works fairly well for the S6400 series sensors when measuring voltage with a multimeter or oscilloscope. But for the S9702, the responsiveness was not as great and so higher resistor values were needed to amplify the signal. However, at such high resistances, it was over the input impedance of my measuring devices, so my measurements were not valid. And even if the measuring devices had a higher input impedance, the input pin of the micro controller only has a 2.5K ohm input impedance.

An interesting find from this attempt was that for sensing ambient room light, the resistor values needed to be higher for green, and even higher for blue in order to get the full voltage swing of response.

Circuit diagram for attempt 2.
Circuit diagram for attempt 3.

Attempt 2

To combat the high impedance of the output, I tried to use an op-amp as a voltage follower. However, I got some strange periodic behavior in the voltage output and also some strange voltage off-set. It would read from -0.5V-ish to 2.7V-ish, a -0.5 off-set. Other more complicated circuitry were also recommended and tried by Prof. Lynch and Prof. Peshkin, but none really worked very well for me.

Attempt 3

This was the third and final attempted circuitry. I used a simple transistor to give the output roughly a 200x current gain, which allowed me to use a much smaller resistor to lower the output impedance. Although I did not look into this in great detail, it seems like there was a huge variance in the amount of current gain between individual transistors. Furthermore, different resistor values work better under different light conditions. So a trim=pot might be a better design if you plan on reproducing this circuit.

Code

Overview

The code listed here will allow the e-Puck to sync up with the MATLAB program, calibrate the sensor, start the lawn mowing sweep, and update position/heading coordinates. Sensor readings along with the location are taken every 200ms and sent back to the PC only when the e-Puck is going in a straight line. Much of the code was programmed in the same fashion as the standard library of e-Puck codes and NUtest.

Code for e-Puck

Code for MATLAB

Results

Code

Full source code

Overview Comments

/*////////////////////////////////////////////////////////////////////////////////////////////////////////////
stiffness_control.c, j.yeung, a.care, e.nickel 2008-01-31

This code is to be used with the programmable stiffness joint.

OVERVIEW
A reset routine is run when the program starts that will run the Fauhaber motor until the slider hits the
switch.  The reset routine will then reset the encoder count to 0.  Then the program polls the analog input
to see what stiffness level the knob is turned to.  If the desired stiffness level is different from the
current stiffness level, the program will move the slider to the desired level.  The program also controls
the worm drive motor passively as the slider moves to ensure that there is enough slack for the slider to
move and also to tighten up the slack after the slider has reached its position.

NOTES
RD0 - binary output for 7-segment display
RD1 - binary output for 7-segment display
RD2 - binary output for 7-segment display
RD3 - binary output for 7-segment display
RD6 - digital output for encoder miscounts
RD7 - digital output for calibration button
RC0 - digital input for calibration button
RC1 - digital output for PWM of rod motor (low = down, high = up)
RC2 - CCP1 output for PWM of rod motor
RB3 - CCP2 output for PWM of cable motor
RB4 - digital output for PWM of cable motor (low = in, high = out)
RA0 - analog input from dial
RA3 - channel B input from rod motor encoder
RA4 - channel A input from rod motor encoder
/////////////////////////////////////////////////////////////////////////////////////////////////////////////*/

PIC Initialization

//PIC Initialization///////////////////////////
#include <18f4520.h>                         // command to include the header for the PIC.
#DEVICE ADC=10                               // set ADC (analog input) to 10 bit accuracy
#fuses HS, NOLVP, NOWDT, NOPROTECT, CCP2B3   // some standard fuses, CCP2B3 moves CCP2 to output at RB3
#use delay(clock=20000000)                   // 20 MHz crystal on PCB
///////////////////////////////////////////////

Encoder Control

//Encoder Control//////////////////////////////
signed int32 encoder_count=0;                // it is signed so just in case it goes negative, it doesn't overflow.
int8 encoder_state=0;                        // will contain only 4 relevant bits: A & B state last time, and this time
                                             //
signed int decoder_lookup[16] = {0, -1, +1, 0, +1, 0, 0, -1, -1, 0, 0, +1, 0, +1, -1, 0};
signed int32 target[10] = {100, 2750, 5500, 8250, 11000, 13750, 16500, 19250, 22000, 24650};
                                             // decoder_lookup is to determine which way the motor is going
                                             // target is to determine what is the target encoder count for each stiffness level.
                                             //
#INT_TIMER2                                  // designates that this is the routine to call when timer2 overflows
void Timer2isr() {                           // see ServoSkeleton for how this ISR can interrupt others
                                             //
   encoder_state = ((((encoder_state << 1) + input(PIN_A4)) << 1) + input(PIN_A3)) & 15; 
                                             // shift over the old states and record the new states
                                             //
   encoder_count += decoder_lookup[encoder_state];
                                             // use decoder_lookup table to determine motor direction and update encoder_count
                                             //
   if ( ((encoder_state & 8) >> 3) != ((encoder_state & 2) >> 1) 
         && ((encoder_state & 4) >> 2) != ((encoder_state & 1) >> 0)) {
      output_high(PIN_D6);                   // turn on D6 LED if there is a miscount
   }                                         //
   else{                                     //
      output_low(PIN_D6);                    // turn off D6 LED if there is no miscount
   }                                         //
}                                            //
///////////////////////////////////////////////

Reset Routine

//Reset Routine////////////////////////////////
void reset(){                                // only called at the beginning of program
   int i=8;                                  //
   int j=0;                                  //
                                             //
   output_low(PIN_C1);                       // set rod motor direction to down
   set_pwm1_duty(78);                        //
   output_high(PIN_B4);                      // set cable motor direction to reel-out
   set_pwm2_duty(0);                         //
   delay_ms(150);                            // let cable motor reel-out for 150ms
   output_low(PIN_B4);                       // stop cable motor
                                             //
   while(input(PIN_C0) == 1){                // keep moving the slider down until the limit switch is closed
      if (i==8){                             // for every 4 seconds
         output_high(PIN_B4);                // let cable motor reel-out for 75ms
         if(j>40){                           // if rod motor has been running for more than 20 seconds
            delay_ms(50);                    // only let cable motor reel-out for 50ms
         }                                   //
         else{                               //
            delay_ms(75);                    //
         }                                   //
         output_low(PIN_B4);                 //
         i=0;                                //
      }                                      //
      delay_ms(500);                         //
      i++;                                   //
      j++;                                   //
   }                                         //
                                             //
   set_pwm1_duty(0);                         // stop rod motor
   output_low(PIN_B4);                       // set cable motor direction to reel-in
   set_pwm2_duty(78);                        // reel-in for 1500ms
   delay_ms(1500);                           //
   set_pwm2_duty(0);                         // stop cable motor
   encoder_count = 50;                       // reset encoder counter
   delay_ms(1000);                           // wait for 1 second
}                                            //
///////////////////////////////////////////////

Change Stiffness

//Change Stiffness/////////////////////////////
signed int8 level_old=0;                     // these are signed to allow for math operations in the function
signed int8 level_new=0;                     //
                                             //
void change_stiffness(){                     // only called when there's a change in desired stiffness level
   int i=0;                                  //
                                             //
   if(level_new - level_old < 0){            // if desired level is lower than previous level
      output_high(PIN_B4);                   // set cable motor direction to reel-out
      set_pwm2_duty(0);                      // reel-out for 500ms
      delay_ms(500);                         //
      set_pwm2_duty(78);                     // stop cable motor
                                             //
      while(encoder_count > target[level_new]){
                                             // until the encoder count reaches the new desired count
         if(i == 100){                       // for every 5 seconds
            output_high(PIN_B4);             // set cable motor direction to reel-out
            set_pwm2_duty(0);                // reel-out for 50ms
            delay_ms(50);                    //
            set_pwm2_duty(78);               // stop cable motor
            i = 0;                           //
         }                                   //
         output_low(PIN_C1);                 // set rod motor direction to down
         set_pwm1_duty(78);                  // run rod motor
         delay_ms(50);                       //
         i++;                                //
      }                                      //
      set_pwm1_duty(0);                      // stop rod motor after target has been reached
   }                                         //
                                             //
   else{                                     // desired level is higher than previous level
      output_high(PIN_B4);                   // set cable motor direction to reel-out
      set_pwm2_duty(0);                      // reel-out for 100ms
      delay_ms(100);                         //
      set_pwm2_duty(78);                     // stop cable motor
                                             //
      while(encoder_count < target[level_new]){
                                             // until the encoder count reaches the new desired count
         if(i == 100){                       // for every 5 seconds
            output_low(PIN_B4);              // set cable motor direction to reel-in
            set_pwm2_duty(78);               // reel-in for 50 ms
            delay_ms(50);                    //
            set_pwm2_duty(0);                // stop cable motor
            i = 0;                           //
         }                                   //
         output_high(PIN_C1);                // set rod motor direction to up
         set_pwm1_duty(0);                   // run rod motor
         delay_ms(50);                       //
         i++;                                //
      }                                      //
      set_pwm1_duty(78);                     // stop rod motor after target has been reached
   }                                         //
                                             // after desired level is reached, tighten slack
   output_low(PIN_B4);                       // set cable motor direction to reel-in
   set_pwm2_duty(78);                        // reel-in for 1500ms
   delay_ms(1500);                           //
   set_pwm2_duty(0);                         // stop cable motor
}                                            //
///////////////////////////////////////////////

Vref Update

//Vref_update//////////////////////////////////
int16 Vref_old=0;                            // need 16 bit because we chose 10 bit accuracy
int16 Vref_new=0;                            // 
                                             //
void Vref_update(){                          // routine to update dial/display and call change_stiffness()
   Vref_new = read_adc();                    // update dial value
                                             //
   if (abs(Vref_new - Vref_old) > 30){       // software schmitt trigger
      if (Vref_new < 1023){                  //
         level_new = 0;                      //
      }                                      //
      if (Vref_new < 918){                   //
         level_new = 1;                      //
      }                                      //
      if (Vref_new < 816){                   //
         level_new = 2;                      //
      }                                      //
      if (Vref_new < 714){                   //
         level_new = 3;                      //
      }                                      //
      if (Vref_new < 612){                   //
         level_new = 4;                      //
      }                                      //
      if (Vref_new < 510){                   //
         level_new = 5;                      //
      }                                      //
      if (Vref_new < 408){                   //
         level_new = 6;                      //
      }                                      //
      if (Vref_new < 306){                   //
         level_new = 7;                      //
      }                                      //
      if (Vref_new < 204){                   //
         level_new = 8;                      //
      }                                      //
      if (Vref_new < 102){                   //
         level_new = 9;                      //
      }                                      //
      output_d(level_new);                   // update value on 7-segment display
      if(level_new != level_old){            // 
         change_stiffness();                 // change stiffness to new desired level
         level_old = level_new;              // update level_old
         Vref_old = Vref_new;                // update Vref_old
      }                                      //
   }                                         //
}                                            //
///////////////////////////////////////////////

Main Function

//MAIN/////////////////////////////////////////
void main(){                                 //
                                             //
   setup_timer_2(T2_DIV_BY_4, 77, 16);       // 16KHz clock, interrupt every 4*50nS * 4 * (77+1) * 16 = 1.0mS
                                             // third argument means the clock is running 16x faster than the interrupt routine, if any
   enable_interrupts(INT_TIMER2);            // initialization for ISR
   enable_interrupts(GLOBAL);                //
                                             //
   setup_ccp1(CCP_PWM);                      // PWM output on CCP1/RC2, pin 17 
   setup_ccp2(CCP_PWM);                      // PWM output on CCP2/RB3, pin 36.
                                             //
   output_high(PIN_C1);                      // initialize rod motor
   set_pwm1_duty(78);                        //
                                             //
   output_high(PIN_B4);                      // initialize cable motor
   set_pwm2_duty(78);                        //
                                             //
   setup_adc_ports(AN0);                     // Enable analog inputs; choices run from just AN0, up to AN0_TO_AN11
   setup_adc(ADC_CLOCK_INTERNAL);            // the range selected has to start with AN0
                                             //
   set_adc_channel(0);                       // there's only one ADC so select which input to connect to it; here pin AN0
   delay_us(10);                             // wait 10uS for ADC to settle to a newly selected input
   Vref_old = read_adc();                    // now you can read ADC as frequently as you like
                                             //
   reset();                                  // run the reset routine
                                             //
   while (TRUE) {                            //
      Vref_update();                         // update the dial
      if(input(PIN_C0) == 1){                // turns on D7 LED if the limit switch is pressed, only for visual reference
         output_high(PIN_D7);                //
      }                                      //
      else{                                  //
         output_low(PIN_D7);                 //
      }                                      //
      delay_ms(100);                         //
   }                                         //
}                                            //
///////////////////////////////////////////////

Results and Reflections

Overall Results

SteelToeResultsPic.JPG

Overall, our project was a great success. There were things that could have been done differently and can be improved, but we managed to achieve what we have set out to do, which was to make a device that models a joint and have the ability to change its rotational stiffness. We hope that "Steel Toe" will aid researchers in creating better prosthetic legs and joints for people in need.

In terms of obtaining some quantitative results, by testing to see how much horizontal force was needed to pull the shank to 90 degrees, we found that "Steel Toe" was able to achieve a factor of about 5 in the range of stiffnesses. Here's a table of our test results.

Stiffness LevelForce Needed to Get to 90 Degrees
02.943 N
13.434 N
24.415 N
35.886 N
47.358 N
59.810 N
611.282 N
712.263 N
813.734 N
914.715 N

Possible Mechanical Improvements

  • Move circuitry to shank.
  • Reduce weight of foot plate.
  • Use faster motor for spinning the rod (or gear up current motor) to increase adjustment speed.

Possible Electrical/Software Improvements

  • Have the program continuously check for changes in dial. Currently it is only checking when motors are not moving.
  • Use a different H-bridge chip to replace existing diodes. L293D instead of L293B.

Future Directions

The actual stiffness of the joint is not constant over the range of motion. For this limited range of motion, a linear approximation is valid, however more precise computations accounting for the non-linearity could be of use.

This device could be modified in design to incorporate more powerful motors to drive the slider faster and even possibly during loading to achieve a stiffness which can change during the gait cycle to adapt to conditions or simply better conform to natural gait kinetics.

Following this device with a model incorporating programmable damping would be another step toward differentiating between the elastic stiffness and damping properties of the ankle joint during walking.