PDA

View Full Version : problem with RoboTurret



rogerpel
10-25-2012, 10:11 PM
hi

I bought a RoboTurret kit with Wii Nunchuk Joystick Control (http://www.trossenrobotics.com/p/Nunchuk-Joystick-Kit.aspx)

card: Seeeduino Mega v1.23
software: Arduino IDE v1.0.1
library: RoboTurretNunChuk.pde

my problem is,
when i upload RoboTurretNunChuk.pde library

my pan and tilt motor, not stop moving (left, right, up, down)

#include <Wire.h>
#include <ArduinoNunchuk.h>
#include <Servo.h>
#define BAUDRATE 19200 //define baudrate for optional serial debugging output
#define JOY_DEADZONE 10 //defines deadzone of joystick
#define ACC_DEADZONE 10 //defines deadzone of joystick
#define JOY_SEN 20 //defines sensitivity factor of joystick. Lower value = more sensitive, higher value = less sensitive
#define ACC_SEN 50 //defines sensitivity factor of accelerometer. Lower value = more sensitive, higher value = less sensitive
#define numaccelReadings 10 //determines the number of values to average. Increasing this will increase smoothing also increase latency.
#define numjoyReadings 5 //determines the number of values to average. Increasing this will increase smoothing also increase latency.
#define laserPin 9 // Laser goes to DIO 9
#define PanID 10 // Pan Servo goes to DIO 10
#define TiltID 11 // Tilt Servo goes to DIO 11
ArduinoNunchuk nunchuk = ArduinoNunchuk(); // Instantiate Arduino Nunchuk library

Servo PanServo; // create servo object to control a servo
Servo TiltServo; // create servo object to control a servo
int laserPower = LOW; // laser on/off state variable
int previousCbutState = 0; // used for on/off state code
// Nunchuk sensor variables
int accx,accy;
byte zbut,cbut;
int joyx,joyy;
// Pan & Tilt variables
byte pan = 90;
byte tilt = 90;
int pan_temp;
int tilt_temp;
// Filtering code variables
int accx_readings[numaccelReadings]; // the readings from the accelerometer X input
byte accx_index = 0; // the index of the current reading
int accx_total = 0; // the running total
int accx_average = 0; // the average
int accy_readings[numaccelReadings]; // the readings from the accelerometer Y input
byte accy_index = 0; // the index of the current reading
int accy_total = 0; // the running total
int accy_average = 0; // the average
int joyx_readings[numjoyReadings]; // the readings from the joystick X input
byte joyx_index = 0; // the index of the current reading
int joyx_total = 0; // the running total
int joyx_average = 0; // the average
int joyy_readings[numjoyReadings]; // the readings from the joystick Y input
byte joyy_index = 0; // the index of the current reading
int joyy_total = 0; // the running total
int joyy_average = 0; // the average
void setup()
{
Serial.begin(BAUDRATE);
nunchuk.init(); // Initialize Nunchuk
PanServo.attach(PanID); //Assigning servo channels
TiltServo.attach(TiltID);
PanServo.write(pan); // centers the servos
TiltServo.write(tilt);
pinMode(laserPin, OUTPUT); // Setting laser DIO to output
Serial.print("RoboTurret Ready\n"); // Send message to serial port
delay(1000);

//set everything in the filter code to 0
for (int accx_Reading = 0; accx_Reading < numaccelReadings; accx_Reading++)
accx_readings[accx_Reading] = 0;
for (int accy_Reading = 0; accy_Reading < numaccelReadings; accy_Reading++)
accy_readings[accy_Reading] = 0;
for (int joyx_Reading = 0; joyx_Reading < numjoyReadings; joyx_Reading++)
joyx_readings[joyx_Reading] = 0;
for (int joyy_Reading = 0; joyy_Reading < numjoyReadings; joyy_Reading++)
joyy_readings[joyy_Reading] = 0;
}
void loop()
{
//Update Nunchuk variables from sensor
GetNunchukValues();
// Laser Toggle on/off code
if (cbut == 1 && previousCbutState == 0){
if (laserPower == HIGH)
laserPower = LOW;
else
laserPower = HIGH;
}
digitalWrite(laserPin, laserPower);
previousCbutState = cbut;
// default joystick/pan-tilt behavior
if (zbut == 0)
{
if(joyx > JOY_DEADZONE || joyx < -JOY_DEADZONE){ // If joystick goes beyond deazone centerpoint...
pan_temp += joyx/JOY_SEN; // Increment temporary variable based upon how far away from center we are
}
if(joyy > JOY_DEADZONE || joyy < -JOY_DEADZONE){
tilt_temp += joyy/JOY_SEN;
}
//keeping values in easily manageable range
if(pan_temp > 128){
pan_temp = 128;
}
else if(pan_temp < -128){
pan_temp = -128;
}
if(tilt_temp > 128){
tilt_temp = 128;
}
else if(tilt_temp < -128){
tilt_temp = -128;
}
pan = map(pan_temp, -128, 128, 0, 180); // scale it to use it with the servo (value between 0 and 180)
tilt = map(tilt_temp, -128, 128, 0, 180); // scale it to use it with the servo (value between 0 and 180)
}
// when zbutton is held down, accelerometer is used to control pan/tilt
else if (zbut == 1)
{
if(accx > ACC_DEADZONE || accx < -ACC_DEADZONE){ // If accelerometer goes beyond deazone centerpoint...
pan_temp += accx/ACC_SEN; // Increment temporary variable based upon how far away from center we are
}
if(accy > 10 || accy < -10){
tilt_temp -= accy/ACC_SEN;
}
//keeping values in easily manageable range
if(pan_temp > 128){
pan_temp = 128;
}
else if(pan_temp < -128){
pan_temp = -128;
}
if(tilt_temp > 128){
tilt_temp = 128;
}
else if(tilt_temp < -128){
tilt_temp = -128;
}
pan = map(pan_temp, -128, 128, 0, 180); // scale it to use it with the servo (value between 0 and 180)
tilt = map(tilt_temp, -128, 128, 0, 180); // scale it to use it with the servo (value between 0 and 180)
}
//update servo values
PanServo.write(pan); // sets the servo position according to the scaled value
TiltServo.write(tilt); // sets the servo position according to the scaled value
delay(15); // waits for the servo to get there
// SerialNunchuk(); //Uncomment for data debugging
}
void GetNunchukValues()
{
nunchuk.update(); //update from Nunchuk library
zbut = nunchuk.zButton;
cbut = nunchuk.cButton;
// We recommend using Accel_Filter at all times, it helps tame the very noisy accelerometer data. We recommend using Joystick_Filter on the Direct Control sketch, but not the Incremental Control Sketch.
// Accel(); //Updates unfiltered values from Joystick. Comment out Accel_Filter(); if you are using this.
Accel_Filter(); //Filters and updates values from Accelerometer. Comment out Accel(); if you are using this.
Joystick(); //Updates unfiltered values from Accelerometer. Comment out Joystick_Filter(); if you are using this.
// Joystick_Filter(); //Filters and updates values from Joystick. Comment out Joystick(); if you are using this.
}
void Joystick() // Unfiltered data. Makes 0 the centerpoint. Range from approx -128 to 128
{
joyx = nunchuk.analogX - 128;
joyy = nunchuk.analogY - 128;
}
void Accel() // Unfiltered data. Makes 0 the centerpoint. Range from approx -215 to 215
{
accx = nunchuk.accelX - 512;
accy = nunchuk.accelY - 512;
}
void Joystick_Filter() //Filtered Joystick Data & Updates
{
// smoothing code
// subtract the last reading:
joyx_total= joyx_total - joyx_readings[joyx_index];
joyy_total= joyy_total - joyy_readings[joyy_index];
// read from the sensor:
joyx_readings[joyx_index] = nunchuk.analogX;
joyy_readings[joyy_index] = nunchuk.analogY;
// add the reading to the total:
joyx_total = joyx_total + joyx_readings[joyx_index];
joyy_total = joyy_total + joyy_readings[joyy_index];
// advance to the next position in the array:
joyx_index = joyx_index + 1;
joyy_index = joyy_index + 1;
// if we're at the end of the array...
if (joyx_index >= numjoyReadings)
// ...wrap around to the beginning:
joyx_index = 0;
// if we're at the end of the array...
if (joyy_index >= numjoyReadings)
// ...wrap around to the beginning:
joyy_index = 0;
// calculate the average:
joyx_average = joyx_total / numjoyReadings;
joyy_average = joyy_total / numjoyReadings;
//write average to accelerometer value variable
joyx = joyx_average - 128; // makes 0 the centerpoint. Range from approx -128 to 128
joyy = joyy_average - 128;
}
void Accel_Filter() //Filtered Accelerometer Data & Updates
{
// smoothing code
// subtract the last reading:
accx_total= accx_total - accx_readings[accx_index];
accy_total= accy_total - accy_readings[accy_index];
// read from the sensor:
accx_readings[accx_index] = nunchuk.accelX;
accy_readings[accy_index] = nunchuk.accelY;
// add the reading to the total:
accx_total = accx_total + accx_readings[accx_index];
accy_total = accy_total + accy_readings[accy_index];
// advance to the next position in the array:
accx_index = accx_index + 1;
accy_index = accy_index + 1;
// if we're at the end of the array...
if (accx_index >= numaccelReadings)
// ...wrap around to the beginning:
accx_index = 0;
// if we're at the end of the array...
if (accy_index >= numaccelReadings)
// ...wrap around to the beginning:
accy_index = 0;
// calculate the average:
accx_average = accx_total / numaccelReadings;
accy_average = accy_total / numaccelReadings;
//write average to accelerometer value variable
accx = accx_average - 512; // makes 0 the centerpoint. Range from approx -215 to 215
accy = accy_average - 512;
}
void SerialNunchuk() // Serial output of raw Nunchuk sensor values for debugging
{
Serial.print(nunchuk.analogX, DEC);
Serial.print(' ');
Serial.print(nunchuk.analogY, DEC);
Serial.print(' ');
Serial.print(nunchuk.accelX, DEC);
Serial.print(' ');
Serial.print(nunchuk.accelY, DEC);
Serial.print(' ');
Serial.print(nunchuk.accelZ, DEC);
Serial.print(' ');
Serial.print(nunchuk.zButton, DEC);
Serial.print(' ');
Serial.println(nunchuk.cButton, DEC);
delay(10);
}