-
Notifications
You must be signed in to change notification settings - Fork 7
/
GPIOpinsMotorConstroller.h
153 lines (119 loc) · 4.01 KB
/
GPIOpinsMotorConstroller.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
#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);
}
}