Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
0 parents
commit 2a3f935
Showing
13 changed files
with
2,098 additions
and
0 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,153 @@ | ||
#define WHEEL 10 | ||
#include "pins.h" | ||
#include <lcd.h> | ||
#include <wiringPi.h> | ||
#include <stdio.h> | ||
#include <stdlib.h> | ||
#include <fcntl.h> | ||
#include <string.h> | ||
#include <sys/ioctl.h> | ||
#include <sys/types.h> | ||
#include <sys/stat.h> | ||
#include <unistd.h> | ||
|
||
|
||
|
||
|
||
void setAcceleration(void); | ||
|
||
void setMotorSpeeds( char speed, int leftWheelVelocity, int rightWheelVelocity,char WHEELGAIN,int turn); | ||
|
||
int fd; // File descrition | ||
char *fileName = "/dev/i2c-1"; // Name of the port we will be using | ||
int address = 0x58; // Address of MD22 shifted right one bit | ||
unsigned char buf[10]; // Buffer for data being read and written on the i2c bus | ||
int lcd; | ||
|
||
|
||
void shutBcmDown() | ||
{ | ||
digitalWrite (STOP_START, LOW); | ||
digitalWrite (RESET_GYRO, LOW); | ||
digitalWrite (RF1, LOW); | ||
digitalWrite (RF2, LOW); | ||
digitalWrite (RF3, LOW); | ||
digitalWrite (RF4, LOW); | ||
digitalWrite (LCD_BLUE, LOW); | ||
digitalWrite (LCD_GREEN, LOW); | ||
digitalWrite (LCD_RED, LOW); | ||
} | ||
void setUpMotor() | ||
{ | ||
if ((fd = open(fileName, O_RDWR)) < 0) { // Open port for reading and writing | ||
printf("Failed to open i2c port\n"); | ||
exit(1); | ||
} | ||
|
||
if (ioctl(fd, I2C_SLAVE, address) < 0) { // Set the port options and set the address of the device we wish to sp$ | ||
printf("Unable to get bus access to talk to slave\n"); | ||
exit(1); | ||
} | ||
buf[0] = 0; | ||
buf[1] = 1; | ||
if ((write(fd, buf, 2)) != 2) { // set mode to 1 -128 (Full Reverse) 0 (Stop) 127 (Full Forward). | ||
printf("Error writing to i2c slave\n"); | ||
exit(1); | ||
} | ||
|
||
} | ||
int SetPinsOut() | ||
{ | ||
if (wiringPiSetup () == -1) | ||
exit (1) ; | ||
|
||
pinMode (STOP_START, INPUT) ; | ||
pinMode (RESET_GYRO, INPUT) ; | ||
pinMode (RF1, INPUT) ; | ||
pinMode (RF2, INPUT) ; | ||
pinMode (RF3, INPUT) ; | ||
pinMode (RF4, INPUT) ; | ||
pinMode (LCD_BLUE, OUTPUT); | ||
pinMode (LCD_GREEN, OUTPUT); | ||
pinMode (LCD_RED, OUTPUT); | ||
|
||
|
||
lcd = lcdInit (4, 20, 4,LCD_RS,LCD_E,LCD_D4,LCD_D5,LCD_D6,LCD_D7,0,0,0,0); | ||
if ( lcd == -1){ | ||
printf("lcdInit 1 failed\n") ; | ||
exit(1) ; | ||
} | ||
|
||
} | ||
|
||
void setMotorSpeeds(char speed, int leftWheelVelocity, int rightWheelVelocity, char WHEELGAIN,int turn) { | ||
|
||
// printf("LftWV %3i\tRgtWV %3i\t",leftWheelVelocity,rightWheelVelocity); | ||
char buf[3]; | ||
char diff = 0; //leftWheelVelocity - rightWheelVelocity; | ||
char RightFixValue = 0; | ||
char LeftFixValue = 0; | ||
|
||
if (leftWheelVelocity > rightWheelVelocity){ | ||
diff = leftWheelVelocity - rightWheelVelocity; | ||
RightFixValue = diff * WHEELGAIN; | ||
printf("3diff %i", diff); | ||
} | ||
else if (rightWheelVelocity > leftWheelVelocity){ | ||
diff = rightWheelVelocity - leftWheelVelocity; | ||
LeftFixValue = diff * WHEELGAIN; | ||
printf("4diff %i", diff); | ||
} | ||
|
||
|
||
|
||
buf[0] = 1; // Register to set speed of motor 1 , Right motor. | ||
|
||
if((int)speed+(int)RightFixValue > 254) buf[1] = 254; | ||
else if((int)speed+(int)RightFixValue< 0) buf[1] = 0; | ||
else buf[1] = speed+RightFixValue; | ||
if ((write(fd, buf, 2)) != 2) { | ||
printf("Error writing to i2c slave\n"); | ||
exit(1); | ||
} | ||
|
||
buf[0] = 2; // Register to set speed of motor 2 , Left motor. | ||
|
||
|
||
|
||
|
||
if((int)speed+(int)LeftFixValue> 254) buf[1] = 254; | ||
else if((int)speed+(int)LeftFixValue< 0 )buf[1] = 0; | ||
else buf[1] = speed+LeftFixValue; | ||
if ((write(fd, buf, 2)) != 2) { | ||
printf("Error writing to i2c slave\n"); | ||
exit(1); | ||
} | ||
printf("left buf %hhd ", buf[1]); | ||
|
||
} | ||
|
||
|
||
void lcdColor(int color) | ||
{ | ||
|
||
if (color == LCD_BLUE){ | ||
digitalWrite(LCD_GREEN,LOW); | ||
digitalWrite(LCD_BLUE,HIGH); | ||
digitalWrite(LCD_RED,LOW); | ||
} | ||
|
||
if (color == LCD_RED){ | ||
digitalWrite(LCD_GREEN,LOW); | ||
digitalWrite(LCD_BLUE,LOW); | ||
digitalWrite(LCD_RED,HIGH); | ||
} | ||
|
||
if (color == LCD_GREEN){ | ||
digitalWrite(LCD_GREEN,HIGH); | ||
digitalWrite(LCD_BLUE,LOW); | ||
digitalWrite(LCD_RED,LOW); | ||
} | ||
|
||
} | ||
|
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,43 @@ | ||
#ifndef _L3G_h | ||
#define _L3G_h | ||
|
||
|
||
|
||
#define L3GD20_ADDRESS 0x6b | ||
#define L3GD20_ADDRESS_SA0_HIGH (0xD6 >> 1) | ||
#define GYR_ADDRESS (0xD6 >> 1) | ||
|
||
|
||
|
||
#define L3G_WHO_AM_I 0x0F | ||
|
||
#define L3G_CTRL_REG1 0x20 | ||
#define L3G_CTRL_REG2 0x21 | ||
#define L3G_CTRL_REG3 0x22 | ||
#define L3G_CTRL_REG4 0x23 | ||
#define L3G_CTRL_REG5 0x24 | ||
#define L3G_REFERENCE 0x25 | ||
#define L3G_OUT_TEMP 0x26 | ||
#define L3G_STATUS_REG 0x27 | ||
|
||
#define L3G_OUT_X_L 0x28 | ||
#define L3G_OUT_X_H 0x29 | ||
#define L3G_OUT_Y_L 0x2A | ||
#define L3G_OUT_Y_H 0x2B | ||
#define L3G_OUT_Z_L 0x2C | ||
#define L3G_OUT_Z_H 0x2D | ||
|
||
#define L3G_FIFO_CTRL_REG 0x2E | ||
#define L3G_FIFO_SRC_REG 0x2F | ||
|
||
#define L3G_INT1_CFG 0x30 | ||
#define L3G_INT1_SRC 0x31 | ||
#define L3G_INT1_THS_XH 0x32 | ||
#define L3G_INT1_THS_XL 0x33 | ||
#define L3G_INT1_THS_YH 0x34 | ||
#define L3G_INT1_THS_YL 0x35 | ||
#define L3G_INT1_THS_ZH 0x36 | ||
#define L3G_INT1_THS_ZL 0x37 | ||
#define L3G_INT1_DURATION 0x38 | ||
|
||
#endif |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,82 @@ | ||
#ifndef LSM303_h | ||
#define LSM303_h | ||
|
||
#include <stdint.h> | ||
|
||
// register addresses | ||
#define MAG_ADDRESS (0x3C >> 1) | ||
#define ACC_ADDRESS (0x32 >> 1) | ||
#define ACC_ADDRESS_SA0_A_LOW (0x30 >> 1) | ||
#define ACC_ADDRESS_SA0_A_HIGH (0x32 >> 1) | ||
|
||
#define LSM303_CTRL_REG1_A 0x20 | ||
#define LSM303_CTRL_REG2_A 0x21 | ||
#define LSM303_CTRL_REG3_A 0x22 | ||
#define LSM303_CTRL_REG4_A 0x23 | ||
#define LSM303_CTRL_REG5_A 0x24 | ||
#define LSM303_CTRL_REG6_A 0x25 // DLHC only | ||
#define LSM303_HP_FILTER_RESET_A 0x25 // DLH, DLM only | ||
#define LSM303_REFERENCE_A 0x26 | ||
#define LSM303_STATUS_REG_A 0x27 | ||
|
||
#define LSM303_OUT_X_L_A 0x28 | ||
#define LSM303_OUT_X_H_A 0x29 | ||
#define LSM303_OUT_Y_L_A 0x2A | ||
#define LSM303_OUT_Y_H_A 0x2B | ||
#define LSM303_OUT_Z_L_A 0x2C | ||
#define LSM303_OUT_Z_H_A 0x2D | ||
|
||
#define LSM303_FIFO_CTRL_REG_A 0x2E // DLHC only | ||
#define LSM303_FIFO_SRC_REG_A 0x2F // DLHC only | ||
|
||
#define LSM303_INT1_CFG_A 0x30 | ||
#define LSM303_INT1_SRC_A 0x31 | ||
#define LSM303_INT1_THS_A 0x32 | ||
#define LSM303_INT1_DURATION_A 0x33 | ||
#define LSM303_INT2_CFG_A 0x34 | ||
#define LSM303_INT2_SRC_A 0x35 | ||
#define LSM303_INT2_THS_A 0x36 | ||
#define LSM303_INT2_DURATION_A 0x37 | ||
|
||
#define LSM303_CLICK_CFG_A 0x38 // DLHC only | ||
#define LSM303_CLICK_SRC_A 0x39 // DLHC only | ||
#define LSM303_CLICK_THS_A 0x3A // DLHC only | ||
#define LSM303_TIME_LIMIT_A 0x3B // DLHC only | ||
#define LSM303_TIME_LATENCY_A 0x3C // DLHC only | ||
#define LSM303_TIME_WINDOW_A 0x3D // DLHC only | ||
|
||
#define LSM303_CRA_REG_M 0x00 | ||
#define LSM303_CRB_REG_M 0x01 | ||
#define LSM303_MR_REG_M 0x02 | ||
|
||
#define LSM303_OUT_X_H_M 0x03 | ||
#define LSM303_OUT_X_L_M 0x04 | ||
#define LSM303_OUT_Y_H_M -1 // The addresses of the Y and Z magnetometer output registers | ||
#define LSM303_OUT_Y_L_M -2 // are reversed on the DLM and DLHC relative to the DLH. | ||
#define LSM303_OUT_Z_H_M -3 // These four defines have dummy values so the library can | ||
#define LSM303_OUT_Z_L_M -4 // determine the correct address based on the device type. | ||
|
||
#define LSM303_SR_REG_M 0x09 | ||
#define LSM303_IRA_REG_M 0x0A | ||
#define LSM303_IRB_REG_M 0x0B | ||
#define LSM303_IRC_REG_M 0x0C | ||
|
||
#define LSM303_WHO_AM_I_M 0x0F // DLM only | ||
|
||
#define LSM303_TEMP_OUT_H_M 0x31 // DLHC only | ||
#define LSM303_TEMP_OUT_L_M 0x32 // DLHC only | ||
#define LSM303DLH_OUT_Y_H_M 0x05 | ||
#define LSM303DLH_OUT_Y_L_M 0x06 | ||
#define LSM303DLH_OUT_Z_H_M 0x07 | ||
#define LSM303DLH_OUT_Z_L_M 0x08 | ||
|
||
#define LSM303DLM_OUT_Z_H_M 0x05 | ||
#define LSM303DLM_OUT_Z_L_M 0x06 | ||
#define LSM303DLM_OUT_Y_H_M 0x07 | ||
#define LSM303DLM_OUT_Y_L_M 0x08 | ||
|
||
#define LSM303DLHC_OUT_Z_H_M 0x05 | ||
#define LSM303DLHC_OUT_Z_L_M 0x06 | ||
|
||
|
||
#endif |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,5 @@ | ||
all: | ||
# gcc -o gyro gyro.c -lm -l bcm2835 | ||
# gcc -o gyro -l rt gyro.c -l bcm2835 -lm | ||
gcc -g -o gyro -l rt gyro.c -l wiringPi -lm | ||
# gcc -g -o gyro -l rt gyro.c -l wiringPi -lm `sdl-config --cflags --libs` -lSDL_ttf -lSDL_gfx |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,5 @@ | ||
This code is for a balancing robot using the Raspberry Pi. | ||
More details here; | ||
http://marks-space.com/2013/07/28/pibbot-v2-balancing-robot-using-a-raspberry-pi | ||
|
||
|
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,73 @@ | ||
#include "pins.h" | ||
|
||
/*#define ENCODER_PIN0 0 | ||
#define ENCODER_PIN1 2 | ||
#define ENCODER_PIN2 1 | ||
#define ENCODER_PIN3 16*/ | ||
|
||
volatile long leftEncoderValue; | ||
volatile long rightEncoderValue; | ||
volatile int lastLeftEncoded = 0; | ||
volatile int lastRightEncoded = 0; | ||
|
||
void updateLeftEncoders() | ||
{ | ||
int MSB = digitalRead(ENCODER_PIN0); | ||
int LSB = digitalRead(ENCODER_PIN1); | ||
|
||
int encoded = (MSB << 1) | LSB; | ||
int sum = (lastLeftEncoded << 2) | encoded; | ||
|
||
if(sum == 0b1101 || sum == 0b0100 || sum == 0b0010 || sum == 0b1011) leftEncoderValue--; | ||
if(sum == 0b1110 || sum == 0b0111 || sum == 0b0001 || sum == 0b1000) leftEncoderValue++; | ||
|
||
lastLeftEncoded = encoded; | ||
// printf("lastLeftEncoded %i \t",lastLeftEncoded); | ||
} | ||
void updateRightEncoders() | ||
{ | ||
int MSB = digitalRead(ENCODER_PIN2); | ||
int LSB = digitalRead(ENCODER_PIN3); | ||
|
||
int encoded = (MSB << 1) | LSB; | ||
int sum = (lastRightEncoded << 2) | encoded; | ||
|
||
if(sum == 0b1101 || sum == 0b0100 || sum == 0b0010 || sum == 0b1011) rightEncoderValue++; | ||
if(sum == 0b1110 || sum == 0b0111 || sum == 0b0001 || sum == 0b1000) rightEncoderValue--; | ||
|
||
lastRightEncoded = encoded; | ||
// printf("lastRightEncoded %i \n", lastRightEncoded); | ||
} | ||
|
||
int setupEncoders(void) | ||
{ | ||
|
||
if (wiringPiSetup () < 0) | ||
{ | ||
fprintf (stderr, "Unable to setup wiringPi: %s\n", strerror (errno)) ; | ||
return 1 ; | ||
} | ||
|
||
if (wiringPiISR (ENCODER_PIN0, INT_EDGE_BOTH, &updateLeftEncoders) < 0) | ||
{ | ||
fprintf (stderr, "Unable to setup ISR: %s\n", strerror (errno)) ; | ||
return 1 ; | ||
} | ||
if (wiringPiISR (ENCODER_PIN1, INT_EDGE_BOTH, &updateLeftEncoders) < 0) | ||
{ | ||
fprintf (stderr, "Unable to setup ISR: %s\n", strerror (errno)) ; | ||
return 1 ; | ||
} | ||
|
||
if (wiringPiISR (ENCODER_PIN2, INT_EDGE_BOTH, &updateRightEncoders) < 0) | ||
{ | ||
fprintf (stderr, "Unable to setup ISR: %s\n", strerror (errno)) ; | ||
return 1 ; | ||
} | ||
if (wiringPiISR (ENCODER_PIN3, INT_EDGE_BOTH, &updateRightEncoders) < 0) | ||
{ | ||
fprintf (stderr, "Unable to setup ISR: %s\n", strerror (errno)) ; | ||
return 1 ; | ||
} | ||
} | ||
|
Oops, something went wrong.