E-Puck Color Sensing Project
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
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.
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
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
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 Level | Force Needed to Get to 90 Degrees |
|---|---|
| 0 | 2.943 N |
| 1 | 3.434 N |
| 2 | 4.415 N |
| 3 | 5.886 N |
| 4 | 7.358 N |
| 5 | 9.810 N |
| 6 | 11.282 N |
| 7 | 12.263 N |
| 8 | 13.734 N |
| 9 | 14.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.
