Full code Pic16f877a Gesture Controlled Robot Arm

/* Code for Gesture Controlled Robotic Arm
Uses ADC values from Accelerometer/bend sensors to control servos
Software created PWMs for controlling multiple servos at 50Hz
  By TGT
2014
*/
#include <pic.h>
#include <htc.h>
#define _XTAL_FREQ 20000000 // defining crystal freq needed for __delay_ms()
#define start 15536 //start value needed for timer1 to overflow at 20ms
#define kfilteringfactor 0.1

 #include "uart.h"
__CONFIG(0x3F3A);

 //Declare function prototypes
void statem(void);
void x_axis(void);
void y_axis(void);
void z_axis(void);

 void bendsensor(void);
void grav(void);
void accel(void);

 //Global vairables
int state,highg=500,lowg=300,incr=1;
unsigned int XA,YA,ZA,value,servoX,servoY,servoZ,servoB,bend,bend1,sensor,sensor1,x,y;
unsigned int pwmh=(6100+start),pwml=(1400+start);
unsigned int pos_neutral=(3750),pos_min=(1700),pos_max=(5800); //servo positions (degrees) 90,0,180 
float gravX,moveX; 

 char AA;

 unsigned int get_adc(void)
{
ADON=1; //Turn on ADC
__delay_us(17); //Acquisition time to charge hold capacitor
GO_DONE=1; //start conversion
while(GO_DONE==1); //wait for conversion to complete
ADON=0; //turn off ADC
value=(ADRESH<<8)+ADRESL; //grab 10bit ADC value
return value; //return value
}

 void interrupt ISR(void) //Timer1 Interrupt Service Routine
{
if (TMR1IF) //If timer1 interrupt flag set
{
TMR1IF=0; //clear the interrupt flag
TMR1H=0x3C; //3CB0h = 15536 starting value to
TMR1L=0xB0; //overflow at 20ms period for pwm
{
PORTB=0xFF; //All servos on portB initial high
}
}
}

 int main(void)
{
// Set up AD module
ADCON1=0b10000000; //Rightjustify (8bit result from ADRESL)PORTA Analogue inputs 
TRISA=0b111111; //analogue inputs
TRISE=0b111; //

// Set up other outputs
TRISC=0x00; //LED accelerometer indicators
TRISD=0x00; //LED output
PORTD=0;

 // Initialize timer module
TRISB=0x00; //PORTB output for servos
T1CON=0b00010000; //prescale=1:2 16bit=65535 200ns x 65535 =0.0131072s x 2 =26.2144ms

 TMR1H=0x3C; //3CB0h = 15536 starting values to overflow at 20ms period for pwm
TMR1L=0xB0; //which = 50hz frequency
PEIE=1; //enable peripheral interrupts
GIE=1; //enable global interrupts
TMR1IE=1; //enable timer1 interrupt

state=0; //sets initial case value
TMR1ON=1; //starts timer1
 
/*----------------------------------------------------------------
Servo calculations
180 degrees: 2ms / [200ns x 2] = 5000 or 2.4ms = 6000
90 degrees : 1.5ms / [200ns x 2] = 3750
0 degrees : 1ms / [200ns x 2] = 2500 or 600us = 1500
------------------------------------------------------------------*/
servoZ=pos_neutral;
servoY=pos_neutral;
servoX=pos_neutral;
servoB=pos_neutral;

 while(1)
{
if(TMR1>=pwml && TMR1<=pwmh)//Will only enter this code if within certain portion of time. 
//This is so it doesnt tax the mcu which can create jitter on servo
{
//Code dedicated to servos here //checks if pins need to be switched off

 if (TMR1 >=( start+3750) && RB0==1) //Base - forward/back
{
RB0=0; //servo 0 off
}
if (TMR1 >=(start+servoY) && RB1==1) //Elbow - up/down
{
RB1=0; //servo 1 off
}
if (TMR1 >=(start+servoX) && RB2==1) //Base - left/right
{
RB2=0; //servo 2 off
}
if (TMR1 >=(start+servoB) && RB3==1) //Wrist - up/down
{
RB3=0; //servo 3 off
}
if (TMR1 >=(start+5000) && RB4==1) //Gripper - open/close
{
RB4=0; //servo 4 off
}
}
else if(TMR1<pwml || TMR1>pwmh)
{
//dedicated to other other tasks for mcu
//eg. changing servo numbers, get adc from accelerometer etc
statem();

}
}
}

 void statem(void)
{  
switch(state)
{
case 0:

ADCON0=0b10001000; //Sets channel 1 for sample
z_axis(); //go to function for X ADC
state=1; //Sets the next case value
break;

case 1:

ADCON0=0b10010000; //Sets channel 2 for sample
y_axis(); //go to function for Y ADC
state=2; //Sets the next case value
break;

case 2:

 ADCON0=0b10011000; //Sets channel 3 for sample
x_axis(); //go to function for Z ADC
state=3; //Sets the next case value
break;

 case 3:
ADCON0=0b10100000; //Sets channel 4 for sample
get_adc(); //get adc value
sensor=value; //store adc

 x=(900-sensor);
bend=(6000/300*(300-x));

state=4;
break;

 case 4:
ADCON0=0b10101000; //Sets channel 5 for sample
get_adc(); //get adc value
sensor1=value;

 y=(900-sensor1);
bend1=(6000/300*(300-y));

 state=5;
break;

 case 5:
//-----------------------------------------------------
// X AXIS
//-----------------------------------------------------
if(XA>=highg) //If +1g
{
{
servoX=servoX+1; //increase pulse width
if(servoX>pos_max) //if number exceeds 2ms
{
servoX=pos_max;
}
}
}
if(XA<lowg) //If -1g
{
servoX=servoX-1; //decrease pulse width
if(servoX<pos_min)
{
servoX=pos_min;
}
}
if (XA>lowg&&XA<highg) //If between +-1g
{
servoX=servoX; //no change
}
//--------------------------------------------------
// Y AXIS
//--------------------------------------------------
if(YA>=highg) //If +1g
{
servoY=servoY+1; //increase pulse width
if(servoY>pos_max) //if number exceeds 2ms ON time
{
servoY=pos_max;
}
}
if(YA<=lowg) //If -1g
{
servoY=servoY-1; //decrease pulse width
if(servoY<pos_min)
{
servoY=pos_min;
}
}
if (YA>lowg&&YA<highg) //If between +-1g
{
servoY=servoY; //no change
}

 //--------------------------------------------------
// Z AXIS
//--------------------------------------------------
if(ZA>=highg) //If +1g
{
servoZ=servoZ+1; //increase pulse width
if(servoZ>pos_max) //if number exceeds 2ms ON time
{
servoZ=pos_max;
}
} 
if(ZA<=lowg) //If g is -1g
{
servoZ=servoZ-1; //decrease pulse width
if(servoZ<pos_min)
{
servoZ=pos_min;
}
}
if (ZA>lowg&&ZA<highg) //If between +-1g
{
servoZ=servoZ; //no change
}

//----------------------------------------------------
// Bend sensor
//----------------------------------------------------

 if(bend<pos_max&&bend>pos_min)
{
servoB=bend;
}
if(bend>=pos_max)
{
servoB=pos_max;
}
if(bend<=pos_min)
{
servoB=pos_min;
}
// bendsensor();
/*
if(sensor>=950)
{
RC3=1;
servo4=servo4+1;
if(servo4>=pos_max)
{
servo4=pos_max;
}
}
if (sensor<=900)
{
RC3=0;                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                       
servo4=servo4-1;
if(servo4<=pos_min)
{
servo4=pos_min;
}
}
*/
state=0;
break;

default:
break;
}
}

 /*---------------------------------------------------
ADC calculations 
10bit ADC: 2^10=1024, 5v reference voltage
1023/5 x Analogue voltage = Digital value
----------------------------------------------------*/

 void x_axis(void)
{
get_adc(); //get AD value from X axis
XA=value; //Stores AD value in XA
// PORTD=XA; // led array

 if (XA>=highg) //if Digital value > high
{
RC0=1; //LED ON
}
else if (XA<lowg) //else LED off
{
RC0=0;
}
}

 void y_axis(void)
{
get_adc(); //get AD value from Y axis
YA=value; //Stores AD value in YA 

 if (YA>=highg)
{
RC1=1;
}
else if (YA<lowg)
{
RC1=0;
}
}

 void z_axis(void)
{
get_adc(); //get AD value from Z axis
ZA=value; //Stores AD value in ZA

 if (ZA>=highg)
{
RC2=1;
}
else if (ZA<lowg)
{
RC2=0;
}
}

 void grav(void) //function to filter out movement (LOWPASS)
{
gravX=(XA*kfilteringfactor)+(gravX*(1-kfilteringfactor));
}
void accel(void)//function to filter out gravity(HIGHPASS)
{
moveX=XA-(XA*kfilteringfactor)+(gravX*(1-kfilteringfactor));
}

 void bendsensor(void)
{
if(sensor>=950)
{
servoB=servoB+1;
if(servoB>=pos_max)
{
servoB=pos_max;
}
}
if (sensor<=900)
{
servoB=servoB-1;
if(servoB<=pos_min)
{
servoB=pos_min;
}
}
}

Comments

Post a Comment

Popular posts from this blog

Gesture Controlled Robot Arm Using PIC16F877A

Image
Introduction This project uses a wearable gesture device to operate a robotic arm. The robot arm will be controlled by the wearers natural arm movements. Main Components 1 x PIC16f877A  1 x AM3X 3-axis accelerometer - to detect arm movements 2 x Bend / flex sensors - to detect wrist and finger movement  How it works The voltages from the sensors are fed into the ADC pins of the PIC16F877A micro controller. The ADC module converts the analogue voltages into their equivalent digital values. We then use these values to control the servos on our robot arm. Flex sensor The Flex Sensor works in the same way as a variable resistor. Its resistance varies when flexed. You can purchase flex sensors online, but you can also make your own which is what I did for this project. If you want to know more on how to make a flex sensor, check out   Making a flex sensor. This is used in a voltage divider circuit and the output voltage can be calculate...
Read more

HC-SR04 Ultrasonic Sensor code for Distance Measurement

Image
The code written below is for the HC-SR04 Ultrasonic Sensor using the PIC16F877A microcontroller. To start measurement, Trig of SR04 must receive a pulse of high (5V) for at least 10us, this will initiate the sensor will transmit out 8 cycle of ultrasonic burst at 40kHz and wait for the reflected ultrasonic burst. When the sensor detected ultrasonic from receiver, it will set the Echo pin to high (5V) and delay for a period (width) which proportion to distance. To obtain the distance, measure the width (Ton) of Echo pin. Time = Width of Echo pulse, in uS (micro second) ● Distance in centimeters = Time / 58 ● Distance in inches = Time / 148 ● Or you can utilize the speed of sound, which is 340m/s Project Schematic PROJECT CODE For the below code, the measured distance will be displayed in binary on an LED array on PORTC of the microcontroller. Eg. 5cm will = 00000101 You should be able to copy paste it. /****************************...
Read more