Lynxmotion Tech Support

 // if ((ulTimeLastMsg - ulTime) > MAX_DELAY) {
        // if we don't receive any valid message for a long time maybe we should set speeds to zero...
       // rthrust = 0;
     //   lthrust = 0;
   // }

 if ((ulTime - ulTimeLastMsg ) > MAX_DELAY) {

/*
 * WiiChuck-Servo Test with LCD output
 *v1.5 10/25/2011
 *
 *
 */

#include <Wire.h>
//#include <LiquidCrystal.h>
#include "nunchuck_funcs.h"
#define debounce 10 // ms debounce
#define holdTime 2000 // ms hold period: how long to wait for press+hold event
#define HALF_DEADBAND 3 //adjust till desired deadband is achieved
#define XRAW_CENTERED 126 // set to whatever "x joystick value is: " on the serial monitor
#define YRAW_CENTERED 129 // set to whatever "y joystick value is: " on the serial monitor
#define DEBUG false // set to false to stop serial messages


int loop_cnt=0;
int valx,valy,LEDstate;
int throttle, steering;
int xraw, yraw, leftSpd, rightSpd;
int ledPin0 = 9;
int ledPin1 = 10;
int ledPin2 = 11;
int ledPin3 = 13;
byte accx,accy,zbut,cbut,xjoy,yjoy;
//LiquidCrystal lcd(7, 8, 9, 10, 11, 12); // LCD pins

int cbutVal = 0; // value read from button
int cbutLast = 0; // buffered value cbut previous state
int zbutVal = 0;
int zbutLast = 0;
long btnDnTime; // time the button was pressed down
long btnUpTime; // time the button was released
boolean ignoreUp = false; // whether to ignore the button release because the click+hold was triggered
boolean ledVal0 = false; // state of LED 0
boolean ledVal1 = false; // state of LED 1
boolean ledVal2 = false; // state of LED 2
boolean ledVal3 = false; // state of LED 3




void setup() {
 
    {
 
  /*pinMode(LEFT_PWM_PIN, OUTPUT);
  pinMode(RIGHT_PWM_PIN, OUTPUT); 
  pinMode(LEFT_DIRECTION_PIN, INPUT);// sets high impedance on pin, brake
  pinMode(RIGHT_DIRECTION_PIN, INPUT);// sets high impedance on pin, brake
  digitalWrite(LEFT_PWM_PIN, LOW); 
  digitalWrite(RIGHT_PWM_PIN, LOW);*/
 
    nunchuck_init(); // send the initilization handshake
    pinMode(ledPin0, OUTPUT);
    digitalWrite(ledPin0, ledVal0);
    pinMode(ledPin1, OUTPUT);
    digitalWrite(ledPin1, ledVal1);
    pinMode(ledPin2, OUTPUT);
    digitalWrite(ledPin2, ledVal2);
    pinMode(ledPin3, OUTPUT);
    digitalWrite(ledPin3, ledVal3);
   
    delay(1000);
  }
}

void loop(){
  {
    {
      if( loop_cnt > 100 ) { // every 100 msecs get new data
        loop_cnt = 0;

        nunchuck_get_data();

        accx  = nunchuck_accelx(); // ranges from approx 70 - 182
        accy  = nunchuck_accely(); // ranges from approx 65 - 173
        zbut = nunchuck_zbutton(); // 0 open 1 closed
        cbut = nunchuck_cbutton(); // 0 open 1 closed
        xjoy = nunchuck_joyx(); // ranges from aprox 29 - 224 c=127-129
        yjoy = nunchuck_joyy(); // ranges from aprox 29 - 226 c=129-132

        /*lcd.begin(16, 2);
        lcd.print("X:");lcd.print((byte)valx,DEC);lcd.write(0xdf);
        lcd.setCursor(6, 0);
        lcd.print("Y:");lcd.print((byte)valy,DEC);lcd.write(0xdf);
        lcd.setCursor(11, 0);if( cbut == HIGH){ lcd.print(" C");}
        lcd.setCursor(13,0); if( zbut == HIGH){ lcd.print(" Z");}
        lcd.setCursor(0, 1); if (ledVal1 == HIGH){lcd.print("Z Lights");}
        lcd.setCursor(9, 1); if (ledVal2 == HIGH){lcd.print("Z Armed!");
        }//End lcd out */
      }
      loop_cnt++;
      delay(1);
    }
 
  { 
    // capture joystick position
    yraw = yjoy;
    xraw = xjoy;
   
    // scale x and y axis so that both are zeroed at natural center of potentiometer, implement deadband
    if(yraw > YRAW_CENTERED + HALF_DEADBAND)//forward
    {
      throttle = map(yraw, YRAW_CENTERED + HALF_DEADBAND, 226, 0, 255);     
      if(xraw > XRAW_CENTERED + HALF_DEADBAND)//left
      {
        steering = map(xraw, XRAW_CENTERED + HALF_DEADBAND, 224, -0, -255);// reverse pot coordinates to agree with cartesian !verify wii X vals!       
      }
      else if(xraw < XRAW_CENTERED - HALF_DEADBAND)// right
      {
        steering = map(xraw, XRAW_CENTERED - HALF_DEADBAND, 0, 0, 255);// reverse pot coordinates to agree with cartesian  !verify wii X vals!       
      }
      else // in X deadband
      {
        steering = 0;
      }
    }
   
    else if(yraw < YRAW_CENTERED - HALF_DEADBAND)//reverse
    {
      throttle = map(yraw, YRAW_CENTERED - HALF_DEADBAND, 0, -0, -255);     
      if(xraw > XRAW_CENTERED + HALF_DEADBAND)//left
      {
        steering = map(xraw, XRAW_CENTERED + HALF_DEADBAND, 224, -0, -255);// reverse pot coordinates to agree with cartesian       
      }
      else if(xraw < XRAW_CENTERED - HALF_DEADBAND)// right
      {
        steering = map(xraw, XRAW_CENTERED - HALF_DEADBAND, 0, 0, 255);// reverse pot coordinates to agree with cartesian       
      }
      else// in X deadband
      {
        steering = 0;
      }
    }
    else //in Y deadband
    {
      throttle = 0;
      if(xraw > XRAW_CENTERED + HALF_DEADBAND)//left
      {
        steering = map(xraw, XRAW_CENTERED + HALF_DEADBAND, 224, -0, -255);// reverse pot coordinates to agree with cartesian       
      }
      else if(xraw < XRAW_CENTERED - HALF_DEADBAND)// right
      {
        steering = map(xraw, XRAW_CENTERED - HALF_DEADBAND, 0, 0, 255);// reverse pot coordinates to agree with cartesian       
      }
      else// in X deadband
      {
        steering = 0;
      }
    }
 
 
  rightSpd = throttle + steering; //transfer from x,y to l,r
  leftSpd = throttle - steering;
  //leftSpd and rightSpd will contain a number from -255 to 255 that gets updated each time the loop runs.
  //-255 equates to full reverse. 0 equates to anywhere within the deadband.  255 is full forward.
 
  // The next 2 sections can be replaced to use with servos, stepper motors, BLDCs or whatever drive you like.
 
/*  //write direction pin
  if (leftSpd < 0)
  digitalWrite(LEFT_DIRECTION_PIN, LOW);
  else if (leftSpd > 0)
  digitalWrite(LEFT_DIRECTION_PIN, HIGH);
   if (rightSpd < 0)
  digitalWrite(RIGHT_DIRECTION_PIN, LOW);

  else if (rightSpd > 0)
  digitalWrite(RIGHT_DIRECTION_PIN, HIGH);
 
 //write PWM pin
  analogWrite(LEFT_PWM_PIN, abs(leftSpd));
  analogWrite(RIGHT_PWM_PIN, abs(rightSpd));
 */
 
  if (DEBUG)
  {
  Serial.print("xjoy: "); Serial.print((byte)xraw,DEC);
  Serial.print("\tyjoy: "); Serial.print((byte)yraw,DEC);
  Serial.print("\tthrottle: "); Serial.print((byte)throttle,DEC);
  Serial.print("\tsteering: "); Serial.print((byte)steering,DEC);
  Serial.print("\tleftSpd: "); Serial.print((byte)leftSpd,DEC);
  Serial.print("\trightSpd: "); Serial.println((byte)rightSpd,DEC);
 
  delay(1);
  }
//End of steering loop///

//Button press&hold loop///
    {
    // Test for Cbut pressed and store the down time
    if (cbut == HIGH && cbutLast == LOW && (millis() - btnUpTime) > long(debounce)) {
      btnDnTime = millis();}
    cbutVal = cbut;// Read the state of the button
    // Test for button release and store the up time
    if (cbutVal == LOW && cbutLast == HIGH && (millis() - btnDnTime) > long(debounce)) {
      if (ignoreUp == false) eventc1();
      else ignoreUp = false;
      btnUpTime = millis();}
    // Test for button held down for longer than the hold time
    if (cbutVal == HIGH && (millis() - btnDnTime) > long(holdTime)) {
      eventc2();
      ignoreUp = true;
      btnDnTime = millis(); }
    cbutLast = cbutVal;//End of push hold C
   
    // Test for Zbut pressed and store the down time
    if (zbut == HIGH && zbutLast == LOW && (millis() - btnUpTime) > long(debounce)) {
      btnDnTime = millis();}
     zbutVal = zbut;// Read the state of the button
    // Test for button release and store the up time
    if (zbutVal == LOW && zbutLast == HIGH && (millis() - btnDnTime) > long(debounce)) {
      if (ignoreUp == false) eventz1();
      else ignoreUp = false;
      btnUpTime = millis();}
    // Test for button held down for longer than the hold time
    if (zbutVal == HIGH && (millis() - btnDnTime) > long(holdTime)) {
      eventz2();
      ignoreUp = true;
      btnDnTime = millis();}
    zbutLast = zbutVal;//End of push hold Z
  }
 }
 }
 }

// Events to trigger by click and press+hold Zbut

void eventz1(){ //Z press
  ledVal0 = !ledVal0;
  digitalWrite(ledPin0, ledVal0);
}
void eventz2(){ //Z hold
  ledVal1 = !ledVal1;
  digitalWrite(ledPin1, ledVal1);
}
void eventc1(){ //C press
  ledVal2 = !ledVal2;
  digitalWrite(ledPin2, ledVal2);
}
void eventc2(){ //C Hold
  ledVal3 = !ledVal3;
  digitalWrite(ledPin3, ledVal3);
}

/*
  Joystick controlled differential steer mobile robot.
  Author: Dustin Maki
  Date: Sept 1 2011
  GPL License.  Free to use, just give credit where due.
  This program scales the joystick reading to start at 1 just outside the deadband so you do not have a large jump in speed.
  It adjusts to the common case where the centered joystick does not rest at quite the center of the range.
  This was first used with
  Teensy++ v.1 http://www.pjrc.com/teensy/pinout.html
  Solarbotics LM298D H bridge http://www.solarbotics.com/products/k_cmd/
  with transistor on each side to switch the direction as in datasheet.
  Joystick http://www.sparkfun.com/products/9032 
     
*/
#define START_BUTTON 21 // not a momentary switch, a toggle switch will work
#define JOYSTICK_BUTTON_PIN 10 // hold joystick with 'tail' toward you.  side with holes on sparkfun breakout board
#define JOYSTICK_X_PIN A0 //full left is 1023, full right is 0
#define JOYSTICK_Y_PIN A1//full forward is 1023, full backward is 0
#define LEFT_PWM_PIN 14
#define LEFT_DIRECTION_PIN 13
#define RIGHT_PWM_PIN 24
#define RIGHT_DIRECTION_PIN 23
#define HALF_DEADBAND 3 //adjust till desired deadband is achieved
#define XRAW_CENTERED 525 // set to whatever "x joystick value is: " on the serial monitor
#define YRAW_CENTERED 493 // set to whatever "y joystick value is: " on the serial monitor
#define DEBUG true // set to false to stop serial messages

void setup()
{
  if (DEBUG)             
  Serial.begin(38400);
 
  pinMode(LEFT_PWM_PIN, OUTPUT);
  pinMode(RIGHT_PWM_PIN, OUTPUT); 
  pinMode(LEFT_DIRECTION_PIN, INPUT);// sets high impedance on pin, brake
  pinMode(RIGHT_DIRECTION_PIN, INPUT);// sets high impedance on pin, brake
  digitalWrite(LEFT_PWM_PIN, LOW); 
  digitalWrite(RIGHT_PWM_PIN, LOW);
}

int throttle, steering;
int xraw, yraw, leftSpd, rightSpd;

void loop()                     
{
  // start button is a push on, push off type. grounded when on
  // when start button is released, program just runs main loop
  while(digitalRead(START_BUTTON) == LOW)
  {
    if (digitalRead(JOYSTICK_BUTTON_PIN) == LOW)//brake requested
    {
      pinMode(LEFT_DIRECTION_PIN, INPUT);// sets high impedance on pin, brake
      pinMode(RIGHT_DIRECTION_PIN, INPUT);// sets high impedance on pin, brake
   
      if (DEBUG)
      {
        Serial.println("brake applied");
      }
      continue;// will not continue main loop, only while
    }
   
  else //start button pressed and not braked
  { 
    // capture joystick position
    yraw = analogRead(JOYSTICK_Y_PIN);
    xraw = analogRead(JOYSTICK_X_PIN);
   
    // scale x and y axis so that both are zeroed at natural center of potentiometer, implement deadband
    if(yraw > YRAW_CENTERED + HALF_DEADBAND)//forward
    {
      throttle = map(yraw, YRAW_CENTERED + HALF_DEADBAND, 1023, 0, 255);     
      if(xraw > XRAW_CENTERED + HALF_DEADBAND)//left
      {
        steering = map(xraw, XRAW_CENTERED + HALF_DEADBAND, 1023, -0, -255);// reverse pot coordinates to agree with cartesian       
      }
      else if(xraw < XRAW_CENTERED - HALF_DEADBAND)// right
      {
        steering = map(xraw, XRAW_CENTERED - HALF_DEADBAND, 0, 0, 255);// reverse pot coordinates to agree with cartesian       
      }
      else // in X deadband
      {
        steering = 0;
      }
    }
   
    else if(yraw < YRAW_CENTERED - HALF_DEADBAND)//reverse
    {
      throttle = map(yraw, YRAW_CENTERED - HALF_DEADBAND, 0, -0, -255);     
      if(xraw > XRAW_CENTERED + HALF_DEADBAND)//left
      {
        steering = map(xraw, XRAW_CENTERED + HALF_DEADBAND, 1023, -0, -255);// reverse pot coordinates to agree with cartesian       
      }
      else if(xraw < XRAW_CENTERED - HALF_DEADBAND)// right
      {
        steering = map(xraw, XRAW_CENTERED - HALF_DEADBAND, 0, 0, 255);// reverse pot coordinates to agree with cartesian       
      }
      else// in X deadband
      {
        steering = 0;
      }
    }
    else //in Y deadband
    {
      throttle = 0;
      if(xraw > XRAW_CENTERED + HALF_DEADBAND)//left
      {
        steering = map(xraw, XRAW_CENTERED + HALF_DEADBAND, 1023, -0, -255);// reverse pot coordinates to agree with cartesian       
      }
      else if(xraw < XRAW_CENTERED - HALF_DEADBAND)// right
      {
        steering = map(xraw, XRAW_CENTERED - HALF_DEADBAND, 0, 0, 255);// reverse pot coordinates to agree with cartesian       
      }
      else// in X deadband
      {
        steering = 0;
      }
    }
   
  //transfer from x,y to l,r
  leftSpd = throttle + steering;
  rightSpd = throttle - steering;
  //leftSpd and rightSpd will contain a number from -255 to 255 that gets updated each time the loop runs.
  //-255 equates to full reverse. 0 equates to anywhere within the deadband.  255 is full forward.
 
  // The next 2 sections can be replaced to use with servos, stepper motors, BLDCs or whatever drive you like.
 
  //write direction pin
  if (leftSpd < 0)
  digitalWrite(LEFT_DIRECTION_PIN, LOW);
  else if (leftSpd > 0)
  digitalWrite(LEFT_DIRECTION_PIN, HIGH); 
  if (rightSpd < 0)
  digitalWrite(RIGHT_DIRECTION_PIN, LOW);
  else if (rightSpd > 0)
  digitalWrite(RIGHT_DIRECTION_PIN, HIGH);
 
 //write PWM pin
  analogWrite(LEFT_PWM_PIN, abs(leftSpd));
  analogWrite(RIGHT_PWM_PIN, abs(rightSpd));
 
 
  if (DEBUG)
  {
  Serial.print("x joystick value is: ");
  Serial.println(xraw);
  Serial.print("y joystick value is: ");
  Serial.println(yraw);
  Serial.print("throttle is: ");
  Serial.println(throttle);
  Serial.print("steering is: ");
  Serial.println(steering);
  Serial.print("leftSpd is: ");
  Serial.println(leftSpd);
  Serial.print("rightSpd is: ");
  Serial.println(rightSpd);
 
  delay(250);
  }
 }
 }
}

  Serial.print("xjoy: "); Serial.print((byte)xraw,DEC);
  Serial.print("\tyjoy: "); Serial.print((byte)yraw,DEC);
  Serial.print("\tthrottle: "); Serial.print((byte)throttle,DEC);
  Serial.print("\tsteering: "); Serial.print((byte)steering,DEC);
  Serial.print("\tleftSpd: "); Serial.print((byte)leftSpd,DEC);
  Serial.print("\trightSpd: "); Serial.println((byte)rightSpd,DEC);

byte xjoyPrev, yjoyPrev;

         if (DEBUG)
            {
                if ((xjoy != xjoyPrev) || (yjoy != yjoyPrev)) {
                    Serial.print("xjoy: ");
                    Serial.print(xraw,DEC);
                    Serial.print("\tyjoy: ");
                    Serial.print(yraw,DEC);
                    Serial.print("\tthrottle: ");
                    Serial.print(throttle,DEC);
                    Serial.print("\tsteering: ");
                    Serial.print(steering,DEC);
                    Serial.print("\tleftSpd: ");
                    Serial.print(leftSpd,DEC);
                    Serial.print("\trightSpd: ");
                    Serial.println(rightSpd,DEC);
                    xjoyPrev = xjoy;
                    yjoyPrev = yjoy;

                    delay(1);
                }
            }
 

#include <Servo.h>

Servo servo1;
Servo servo2;
int gain1 = 2;
int gain2 = 2;
int s1= 0;
int s2= 0;

void setup()
{
// Serial.begin(115200);
servo1.attach(9); // attach Servo 1 to pin 9
servo2.attach(10); // attach Servo 2 to pin 10
pinMode(3, INPUT); // Signal 1 input at pin3 (Elevator)
pinMode(4, INPUT); // Signal 2 input at pin4 (Rudder)
}

void loop()
{
s1 = (pulseIn( 3,HIGH)-1500)/gain1;
s2 = (pulseIn( 4,HIGH) -1500)/gain2;
servo1.writeMicroseconds(s1+s2+1500);
servo2.writeMicroseconds(s1-s2+1500);
// Serial.println(s1);
}