LeoWhite wrote:
I had similar issues with controlling my T'Rex controller via I2C on the Raspberry Pi, with the Arduino seemingly locking up in the I2C handler code. I tried various things to work around it but, at the time, the best I managed was to enable the hardware watchdog on the Arduino so it would reset after 1/4 second of no activity (Note: I had to update the bootloader on the T'Rex controller first, as the pre-installed one didn't have the watchdog properly enabled).
Leo
Thanks for your reply! I'm glad to know that I'm not the only person who has had this problem... I'm much luckier to have some support though!
pigpio is an excellent solution to the problem! Thanks to your code (after deciphering it--- don't know much C++, but I’m getting there), and to some help from Joan the developer of pigpio, I have the T'rex up and running.
I'm putting this up for anyone who wants it, or just for people who are curious...
My current setup is as follows:
- Simple T'rex arduino sketch (unfortunately not the original--- although I'll be working on that now)
- pigpio on the RPi2.
I've defined specific routines for forward, reverse, right, left, pivoting and stop on the T'rex. The RPi sends 3 bytes (vs the 27 in the original sketch) to work. The first is a startbyte (error checking) with a value of 15 (0x0F), followed by the command byte (forward, reverse, right, left, stop, etc), followed by the speed of the motor. I'm looking into making it much more elegant, but this is what I can do/ am learning to do, at the moment.
T'rex Sketch: (parts of which are from the original T'rex sample code by Oddbot)
Code: Select all
#include <Wire.h> // interrupt based I2C library
#include <Servo.h> // library to drive up to 12 servos using timer1
//#include <EEPROM.h> // library to access EEPROM memory
//#include "IOpins.h"
#define left_encoder_pin 6 // D6 - left motor encoder input - optional
#define right_encoder_pin 5 // D5 - right motor encoder input - optional
#define lm_brk_pin 4 // D4 - left motor brk control pin HIGH = brk
#define lm_dir_pin 2 // D2 - left motor direction control pin HIGH = Forward Low = Reverse
#define lm_pwm_pin 3 // D3 - left motor pulse width modulation pin 0 - 255 Speed and brk
#define lm_cur_pin 6 // A6 - left motor current monitor pin 0 - 1023 -20A to +20A
#define rm_brk_pin 9 // D9 - right motor brk control pin HIGH = brk
#define rm_dir_pin 10 // D10 - right motor direction control pin HIGH = Forward Low = Reverse
#define rm_pwm_pin 11 // D11 - right motor pulse width modulation pin 0 - 255 Speed and brk
#define rm_cur_pin 7 // A7 - right motor current monitor pin 0 - 1023 -20A to +20A
#define voltspin 3 // A3 - battery voltage 1V = 33.57 30V = 1007
#define axisxpin 0 // A0 - accelerometer X-axis
#define axisypin 1 // A1 - accelerometer Y-axis
#define axiszpin 2 // A2 - accelerometer Z-axis
byte slave_address = 7;
//command bytes
byte CMD_FWD = 0x01;
byte CMD_REV = 0x02;
byte CMD_RT = 0x03;
byte CMD_LT = 0x04;
byte CMD_Piv_RT = 0x05;
byte CMD_Piv_LT = 0x06;
byte CMD_STOP = 0x07;
byte CMD_OFF = 0x08;
int LSpeed = 0;
int RSpeed = 0;
int Speed =0 ;
byte startbyte = 0x0F; // start with byte 15
byte errorflag; //dont really know what this is
byte recvflag;
byte cmd;
void setup()
{
Serial.begin(9600);
Serial.print("hello");
TCCR2B = TCCR2B & B11111000 | B00000110;
// Start I2C Bus as Slave
Wire.begin(slave_address);
Wire.onReceive(receiveEvent);
//motor setup
pinMode(lm_pwm_pin,OUTPUT); // configure left motor PWM pin for output
pinMode(lm_dir_pin,OUTPUT); // configure left motor direction pin for output
pinMode(lm_brk_pin,OUTPUT); // configure left motor brk pin for output
pinMode(rm_pwm_pin,OUTPUT); // configure right motor PWM pin for output
pinMode(rm_dir_pin,OUTPUT); // configure right motor direction pin for output
pinMode(rm_brk_pin,OUTPUT); // configure right motor brk pin for output
}
void loop()
{
//Serial.print (Wire.read());
//delay (1000);
}
void receiveEvent(int howMany)
{
byte b;
int i;
//Serial.print(b);
do
{
b=Wire.read();
Serial.println(b);
if(b!=startbyte)errorflag = errorflag | 1;
} while (errorflag>0 &&Wire.available()>0);
if(errorflag>0)
{
Serial.print("error ");
Serial.println(errorflag);
return;
}
b=Wire.read();
if (b>0 && b<9)
{
cmd=b;
Serial.println(b);
}
else
{
errorflag =errorflag |2;
}
i=Wire.read();//*256+Wire.read();
if(i>-1 && i<256)
{
Speed=i;
Serial.println(i);
}
else
{
errorflag =errorflag |3;
}
Serial.println(cmd,Speed);
//delay(50000);
if (cmd == CMD_FWD){
digitalWrite(lm_brk_pin,LOW); // if left brk>0 then engage electronic braking for left motor
digitalWrite(lm_dir_pin,1); // if left speed>0 then left motor direction is forward else reverse
analogWrite (lm_pwm_pin,Speed); // set left PWM to absolute value of left speed - if brk is engaged then PWM controls braking
//if(lmbrk>0 && lmspeed==0) lmenc=0; // if left brk is enabled and left speed=0 then reset left encoder counter
digitalWrite(rm_brk_pin,LOW); // if right brk>0 then engage electronic braking for right motor
digitalWrite(rm_dir_pin,HIGH); // if right speed>0 then right motor direction is forward else reverse
analogWrite (rm_pwm_pin,Speed); // set right PWM to absolute value of right speed - if brk is engaged then PWM controls braking
//if(rmbrk>0 && rmspeed==0) rmenc=0; // if right brk is enabled and right speed=0 then reset right encoder counter
//Serial.println('Forward 100');
}
if (cmd == CMD_REV){
digitalWrite(lm_brk_pin,LOW); // if left brk>0 then engage electronic braking for left motor
digitalWrite(lm_dir_pin,LOW); // if left speed>0 then left motor direction is forward else reverse
analogWrite (lm_pwm_pin,Speed); // set left PWM to absolute value of left speed - if brk is engaged then PWM controls braking
//if(lmbrk>0 && lmspeed==0) lmenc=0; // if left brk is enabled and left speed=0 then reset left encoder counter
digitalWrite(rm_brk_pin,LOW); // if right brk>0 then engage electronic braking for right motor
digitalWrite(rm_dir_pin,LOW); // if right speed>0 then right motor direction is forward else reverse
analogWrite (rm_pwm_pin,Speed); // set right PWM to absolute value of right speed - if brk is engaged then PWM controls braking
//if(rmbrk>0 && rmspeed==0) rmenc=0; // if right brk is enabled and right speed=0 then reset right encoder counter
//Serial.println('Reverse 100');
}
if (cmd == CMD_LT){ //RIGHT
digitalWrite(lm_brk_pin,HIGH); // if left brk>0 then engage electronic braking for left motor
digitalWrite(lm_dir_pin,HIGH); // if left speed>0 then left motor direction is forward else reverse
analogWrite (lm_pwm_pin,0); // set left PWM to absolute value of left speed - if brk is engaged then PWM controls braking
//if(lmbrk>0 && lmspeed==0) lmenc=0; // if left brk is enabled and left speed=0 then reset left encoder counter
digitalWrite(rm_brk_pin,LOW); // if right brk>0 then engage electronic braking for right motor
digitalWrite(rm_dir_pin,HIGH); // if right speed>0 then right motor direction is forward else reverse
analogWrite (rm_pwm_pin,Speed); // set right PWM to absolute value of right speed - if brk is engaged then PWM controls braking
//if(rmbrk>0 && rmspeed==0) rmenc=0; // if right brk is enabled and right speed=0 then reset right encoder counter
//Serial.println('Left 100');
}
if (cmd == CMD_RT){ //LEFT
digitalWrite(lm_brk_pin,LOW); // if left brk>0 then engage electronic braking for left motor
digitalWrite(lm_dir_pin,HIGH); // if left speed>0 then left motor direction is forward else reverse
analogWrite (lm_pwm_pin,Speed); // set left PWM to absolute value of left speed - if brk is engaged then PWM controls braking
//if(lmbrk>0 && lmspeed==0) lmenc=0; // if left brk is enabled and left speed=0 then reset left encoder counter
digitalWrite(rm_brk_pin,HIGH); // if right brk>0 then engage electronic braking for right motor
digitalWrite(rm_dir_pin,HIGH); // if right speed>0 then right motor direction is forward else reverse
analogWrite (rm_pwm_pin,0); // set right PWM to absolute value of right speed - if brk is engaged then PWM controls braking
//if(rmbrk>0 && rmspeed==0) rmenc=0; // if right brk is enabled and right speed=0 then reset right encoder counter
//Serial.println('Right 100');
}
if (cmd == CMD_Piv_RT){
digitalWrite(lm_brk_pin,LOW); // if left brk>0 then engage electronic braking for left motor
digitalWrite(lm_dir_pin,HIGH); // if left speed>0 then left motor direction is forward else reverse
analogWrite (lm_pwm_pin,Speed); // set left PWM to absolute value of left speed - if brk is engaged then PWM controls braking
//if(lmbrk>0 && lmspeed==0) lmenc=0; // if left brk is enabled and left speed=0 then reset left encoder counter
digitalWrite(rm_brk_pin,LOW); // if right brk>0 then engage electronic braking for right motor
digitalWrite(rm_dir_pin,LOW); // if right speed>0 then right motor direction is forward else reverse
analogWrite (rm_pwm_pin,Speed); // set right PWM to absolute value of right speed - if brk is engaged then PWM controls braking
//if(rmbrk>0 && rmspeed==0) rmenc=0; // if right brk is enabled and right speed=0 then reset right encoder counter
//Serial.println('Pivot_right 100');
}
if (cmd == CMD_Piv_LT){
digitalWrite(lm_brk_pin,LOW); // if left brk>0 then engage electronic braking for left motor
digitalWrite(lm_dir_pin,LOW); // if left speed>0 then left motor direction is forward else reverse
analogWrite (lm_pwm_pin,Speed); // set left PWM to absolute value of left speed - if brk is engaged then PWM controls braking
//if(lmbrk>0 && lmspeed==0) lmenc=0; // if left brk is enabled and left speed=0 then reset left encoder counter
digitalWrite(rm_brk_pin,LOW); // if right brk>0 then engage electronic braking for right motor
digitalWrite(rm_dir_pin,HIGH); // if right speed>0 then right motor direction is forward else reverse
analogWrite (rm_pwm_pin,Speed); // set right PWM to absolute value of right speed - if brk is engaged then PWM controls braking
//if(rmbrk>0 && rmspeed==0) rmenc=0; // if right brk is enabled and right speed=0 then reset right encoder counter
//Serial.println('Pivot_left 100');
}
if (cmd == CMD_STOP){
digitalWrite(lm_brk_pin,HIGH); // if left brk>0 then engage electronic braking for left motor
digitalWrite(lm_dir_pin,LOW); // if left speed>0 then left motor direction is forward else reverse
analogWrite (lm_pwm_pin,0); // set left PWM to absolute value of left speed - if brk is engaged then PWM controls braking
//if(lmbrk>0 && lmspeed==0) lmenc=0; // if left brk is enabled and left speed=0 then reset left encoder counter
digitalWrite(rm_brk_pin,HIGH); // if right brk>0 then engage electronic braking for right motor
digitalWrite(rm_dir_pin,LOW); // if right speed>0 then right motor direction is forward else reverse
analogWrite (rm_pwm_pin,0); // set right PWM to absolute value of right speed - if brk is engaged then PWM controls braking
//if(rmbrk>0 && rmspeed==0) rmenc=0; // if right brk is enabled and right speed=0 then reset right encoder counter
//Serial.print("aaahhahahahaaaa --- Stopping");
}
}
On the Raspberry side I've used python and pygame with a PS3-like gamepad controller to control the robot. It's not complete yet, but you get the idea of what I want to do.
Code: Select all
#!/usr/bin/env python
# Tea_rex.py Because we like tea not coffee
# run by doing
# sudo python Tea_rex.py
## Send commands to T'rex Via bit-banged I2C pins on Raspberry pi 2
## Imports
import sys
import smbus
import time
import pygame
#from pygame.locals import *
#bus = smbus.SMBus(1) #Not needed since we are bit banging the I2C Pins using pigpio
import pigpio
# I2C address of Arduino Slave
address = 0x07
#T'rex Bytes defined in Arduino sketch (on T'rex)
CMD_FWD = 0x01; #Forward
CMD_REV = 0x02; #Reverse
CMD_RT = 0x03; #Right Motors Forward
CMD_LT = 0x04; #Left Motors Forward
CMD_Piv_RT = 0x05; #Pivot Left (rt FWD lt REV)
CMD_Piv_LT = 0x06; #Pivot Right (lt FWD rt REV)
CMD_STOP = 0x07; #Stop everything (Brake essentially)
CMD_OFF = 0x08; #Off ... not defined yet
# define pins for bit banging on Pi
# pigpio ALWAYS uses Broadcom GPIO numbering
SDA=2 #this is pin number 3
SCL=3 #this is pin number 5
pi = pigpio.pi() # Connect to Pi.
''' Definitions for bitbanging subroutines follow here
they are called in the while loop: e.g. forward(speed)
the speed is determined by the value given by the potentiometer
on the gamepad, and multiplied by 255 to give a speed that is
passed to the T'rex
'''
def forward(speed):
(count, data) = pi.bb_i2c_zip(SDA, [4, 0x07, 2, 7, 3, 0x0F, CMD_FWD, speed, 3, 0])
print(count, data)
def reverse(speed):
(count, data) = pi.bb_i2c_zip(SDA, [4, 0x07, 2, 7, 3, 0x0F, CMD_REV, speed, 3, 0])
print(count, data)
def right(speed):
(count, data) = pi.bb_i2c_zip(SDA, [4, 0x07, 2, 7, 3, 0x0F, CMD_RT, speed, 3, 0])
def left(speed):
(count, data) = pi.bb_i2c_zip(SDA, [4, 0x07, 2, 7, 3, 0x0F, CMD_LT, speed, 3, 0])
def pivot_right(speed):
(count, data) = pi.bb_i2c_zip(SDA, [4, 0x07, 2, 7, 3, 0x0F, CMD_Piv_RT, speed, 3, 0])
def pivot_left(speed):
(count, data) = pi.bb_i2c_zip(SDA, [4, 0x07, 2, 7, 3, 0x0F, CMD_Piv_LT, speed, 3, 0])
def stop():
(count, data) = pi.bb_i2c_zip(SDA, [4, 0x07, 2, 7, 3, 0x0F, CMD_STOP, 0x00, 3, 0])
#main program
try:
if not pi.connected:
exit(0)
#pigpiod must be running or this won't even load!
pi.bb_i2c_open(SDA, SCL, baud=100000) # Prepare to bit bang.
#initialize pygame
pygame.init()
j = pygame.joystick.Joystick(0)
j.init()
print 'Initialized Joystick : %s' % j.get_name()
print 'number of buttons : %s' % j.get_numbuttons()
## Thank you Pygame##
Keep_playing = True
while (Keep_playing):
for event in pygame.event.get():
if event.type == pygame.QUIT:
print ("Received event 'Quit', exiting.")
keepPlaying = False
elif event.type == pygame.KEYDOWN and event.key == pygame.K_ESCAPE:
print ("Escape key pressed, exiting.")
keepPlaying = False
elif event.type == pygame.KEYDOWN:
print ("Keydown,", event.key)
elif event.type == pygame.KEYUP:
print ("Keyup,", event.key)
#####################################################---Joystick-Axis---###################################################3###3
elif event.type == pygame.JOYAXISMOTION:
if event.dict['axis'] == 0 and event.dict['value'] > 0:
my_percent = event.dict['value']
speed = abs (int(my_percent *255))
right(speed)
print (speed)
print ('Left-joy 0 --- 0 Right', event.dict)
elif event.dict['axis'] == 0 and event.dict['value'] < 0:
my_percent = event.dict['value']
speed = abs (int(my_percent *255))
print (speed)
left(speed)
print ('Left-joy 0 --- 0 Left', event.dict)
elif event.dict['axis'] == 1 and event.dict['value'] > 0:
my_percent = event.dict['value']
speed = abs (int(my_percent *255))
print (speed)
reverse(speed)
print ('Left-joy 1 --- 1 Down', event.dict)
elif event.dict['axis'] == 1 and event.dict['value'] < 0:
my_percent = event.dict['value']
speed = abs (int(my_percent *255))
forward(speed)
print (speed)
print ('Left-joy 1 --- 1 UP', event.dict)
elif event.dict['axis'] == 2 and event.dict['value'] > 0:
# This joy stick will be used to control a servo
my_percent = event.dict['value']
speed = abs (int(my_percent *255))
print (speed)
print ('Right-joy 2 --- 2 Right', event.dict)
elif event.dict['axis'] == 2 and event.dict['value'] < 0:
my_percent = event.dict['value']
speed = abs (int(my_percent *255))
print (speed)
print ('Right-joy 2 --- 2 Left', event.dict)
elif event.dict['axis'] == 3 and event.dict['value'] > 0:
my_percent = event.dict['value']
speed = abs (int(my_percent *255))
print (speed)
print ('Right-joy 3 --- 3 Down', event.dict)
elif event.dict['axis'] == 3 and event.dict['value'] < 0:
my_percent = event.dict['value']
speed = abs (int(my_percent *255))
print (speed)
print ('Right-joy 3 --- 3 UP', event.dict)
#print "Joystick '",j.get_name(),"' axis",event.axis,"motion."
####################################################---Joystick-Buttons-Down (Press-Down)---###########################################
elif event.type == pygame.JOYBUTTONDOWN :
# print "Joystick '",j.get_name(),"' button",event.button,"down.", event.dict
if event.button == 0:
forward(100)
print ('Button 0 pressed ',event.dict)
if event.button == 1:
pivot_right(100)
print ('Button 1 pressed')
if event.button == 2:
print ('Button 2 pressed')
reverse(100)
if event.button == 3:
print ('Button 3 pressed')
pivot_left(100)
#pivot_left(st) ####----------------->Pivot left
if event.button == 4:
stop()
#estop(st)####----------------------------------------------------> EMERGENCY BRAKE
print ('Button 4 pressed ---- EMERGENCY BRAKE!!!!')
#estop()
#time.sleep(0.5)####----------------------------------------->wait 0.3 seconds
if event.button == 5:
print ('Button 5 pressed ---- Taking a Picture!!! ')
#os.system('raspistill --nopreview -t 1 -q 10 -o Rover.jpg -th 0:0:0 &')#####------> Take a Picture! Button 5
if event.button == 6:
print ('Button 6 pressed')
if event.button == 7:
print ('Button 7 pressed')
if event.button == 8:
print ('Button 8 pressed --------- Recording a Video ')
#os.system('raspivid -o myvid.h264 -w 720 -h 405 -t 60000') ##### --------------> Record a Video (1min) Button 8
if event.button == 9:
print ('Button 9 pressed')
#os.system('sudo reboot') ####---------------------------------> Restart Pi Button 9
if event.button == 10:
print ('Button 10 pressed')
if event.button == 11:
print ('Button 11 pressed .... Exiting')
############################################---Joystick-Buttons-UP (Button Release)---###############################################
elif event.type == pygame.JOYBUTTONUP:
#print "Joystick '",j.get_name(),"' button",event.button,"down.", event.dict
if event.button == 0:
print ('Button 0 released ---- BRAKE Lights ON')
stop()
elif event.button == 1:
stop() ##STOP--- BRAKE
print ('Button 1 released')
elif event.button == 2:
stop() ##STOP--- BRAKE
print ('Button 2 released')
elif event.button == 3:
print ('Button 3 released')
stop()#command ##STOP--- BRAKE
elif event.button == 4:
print ('Button 4 released')
stop()#command ##STOP--- BRAKE
elif event.button == 5:
print ('Button 5 released')
stop()#command ##STOP--- BRAKE
elif event.button == 6:
print ('Button 6 released')
stop()#command ##STOP--- BRAKE
elif event.button == 7:
print ('Button 7 released')
stop()#command ##STOP--- BRAKE
elif event.button == 8:
print ('Button 8 released')
elif event.button == 9:
print ('Button 9 released')
#command
elif event.button == 10:
print ('Button 10 released')
#command
elif event.button == 11:
print ('Button 11 pressed .... Exiting') #command
stop()
pygame.quit()
sys.exit()
#################################################--- Hat Buttons (D-Pad controllers) ---#############################################
elif event.type == pygame.JOYHATMOTION:
# print "Joystick '",j.get_name(),"' hat",event.hat," moved.",event.dict
if event.dict['hat'] == 0 and event.dict['value'] == (0, 0):
#estop(st) ###STOP when hat released
print ('D-Pad release !!! ---- Stopping/BRAKE ', event.dict)
elif event.dict['hat'] == 0 and event.dict['value'] == (0, -1):
print ('D-Pad Down ---- Gear DOWN! ', event.dict)
#Possibly change motors pwm setting later
elif event.dict['hat'] == 0 and event.dict['value'] == (0, 1):
print ('D-Pad Up ---- Gear UP! ', event.dict)
#gear_up_FWD(st)
elif event.dict['hat'] == 0 and event.dict['value'] == (-1, 0):
print ('D-Pad Left ---- Turn Left ', event.dict)
#turn_left(st) ####--------------> Turn Left
elif event.dict['hat'] == 0 and event.dict['value'] == (1, 0):
print ('D-Pad Right ---- Turn Right ', event.dict)
#turn_right(st) ####--------------> Turn Right
except:
stop()
pi.bb_i2c_close(SDA) # Stop bit banging I2C.
pi.stop() # Disconnect from Pi.
I haven't quite figured out how to sent the correct number of bytes to the original T'rex sketch, or even how to read the sensors on it (yet). Any bits of code are much appreciated... But for now it's time for me to do some work.
Merry Christmas!
