Previous posts for this project:

http://www.element14.com/community/community/design-challenges/sci-fi-your-pi/blog/authors/screamingtiger?ICID=BLOG-SciFiPi-challengers

 

I have been updating my code for the QuadCop in the GitHub.  I am new to Git Hub and many of my changes were not being committed.

 

https://github.com/screamingtiger/QuadCOP

 

So for anyone looking at the code it may make more sense now.  The ControlSwitch_32 was very outdated and missing the head control logic.

 

On Another note, I have some parts ordered!  I need a new camera and going with the Pi NoirPi Noir.

 

Im busy this week but next week its back on the project! Going to make a video of the sensor array in action, something I didn't get to before the challenge ended.

 

The head code is here.  It basically moves the head left and right, at a random point, at random times.  It only updates every few seconds.  It needs to be a bit more complex but I plan to use a 360 degree servo.

 

Keep in mind this code for the the ChipKit PiChipKit Pi

 

//A function that randomly moves the head around

void MoveHead()
{
  
    
 //SoftPWMServoServoWrite

        if(millis() - headLastUpdate > headDelay)      
 {
  
    if(headDirection == 0)
   headCurrentPWM -= headSpeedFactor;
  else
   headCurrentPWM += headSpeedFactor;

  if(random(1,10000) < 30)
                {
                  headDirection++;
                  headDirection %= 2;
                  headDelay=random(1,3) * 1000;
                  headSpeedFactor = random(1,6);
                  headSpeedFactor =10;
                }
                else  
                  headDelay = 10;
                  
                if(headCurrentPWM >= MAXPWM || headCurrentPWM <= MINPWM)
  {
                    
   if(headDirection == 0)
                        {
                            headDirection = 1;
                            headCurrentPWM = MINPWM + 10;
                        }
                        else
                        {
                            headDirection = 0;
                            headCurrentPWM = MAXPWM - 10;
                        }
                        
   headSpeedFactor = random(1,3) * 10;
;
                        
                        //headCurrentPWM = headStopPointPWM;
                        
                      
                           //Wait up to 3 seconds before moving again
                        headDelay = random(1,3);
                        headDelay *= 1000;
  
  }
                else
                {
                  
                        SoftPWMServoServoWrite(HEADSERVO,headCurrentPWM);
              headLastUpdate = millis();
                }

 }

}

Here is the ControlSwitch code I was using before the challenge ended.  Still messy.

#include <Wire.h>
#include <SoftPWMServo.h>


#define SERVODEADBAND 5
#define READPWMMAXDELAYLOW 30
#define READPWMMAXDELAYHIGH 20
#define MINPWM 1200
#define MAXPWM 2000
/*
PWM output to Flight controller (PWM write)
3 - Rx Channel 1 (Left and Right)
5 - Rx Channel 2 (forward and reverse)
6 - Rx Channel 3 (climb and dive)
9 - Rx Channel 4 (rotate left and right)
*/
//PWM outut from ChipKit to Flight Controller
#define FLIGHTCONTROL_X 0
#define FLIGHTCONTROL_Y 1
#define FLIGHTCONTROL_Z A0
#define FLIGHTCONTROL_R 3
//PWM output to Head Servo
#define HEADSERVO  A1

//PWM input from Rx to Chipkit.
#define RXCHANNEL1 8
#define RXCHANNEL2 9
#define RXCHANNEL3 10
#define RXCHANNEL4 11
#define RXCHANNEL6 13
#define RXCHANNEL5 12
//Output pins to RPi for auto and macro mode switching
#define RPIAUTOMODE   2
//Move MACROMODE to RPI Read
#define RPIMACROMODE  0
/***********************************************************************
 The SPEED definition is the amount to increment or decrement
 the PWM nuetral point in order to move in that direction.  
 In the case it is
 not enough to cuase movement due to wind or other issues,
 the code will increment slowly by FASTER or decrement by SLOWER
 which is a small value.  This will allow the quad to make
 small adjustments in order to compensate without being 
 over controlled. 
***********************************************************************/
#define SPEED  100 
#define STOP            1500
#define STOPHOVER       1800
#define MOVELEFT STOP-SPEED
#define MOVERIGHT STOP+SPEED
#define MOVEFORWARD STOP+SPEED
#define MOVEREVERSE STOP-SPEED
#define ROTATERIGHT STOP-SPEED
#define ROTATELEFT STOP+SPEED

//an integer to indicate directions

//Climbing and diving is done slower
#define MOVECLIMB  STOP+SPEED/2
#define MOVEDIVE  STOP-SPEED/2
#define SPEEDFACTOR 5
#define FASTER  SPEED/SPEEDFACTOR
#define SLOWER  (SPEED/SPEEDFACTOR)*-1

//Adjustment variables for block and controlbyte
#define  FORWARDADJUST 1
#define  REVERSEADJUST 2
#define  LEFTADJUST    3
#define  RIGHTADJUST   4
#define  CLIMBADJUST   5
#define  DIVEADJUST    6
#define  RRIGHTADJUST  7
#define  RLEFTADJUST   8
#define  ADJUSTFASTER  10
#define  ADJUSTSLOWER  20
//Global Variables
//Registers



//Speed Vars
int xSpeed = 0;
int ySpeed = 0;
int zSpeed = 0;
int rSpeed = 0;

//Servo pins for PWM output to the flight controller
bool autoMode = false;
bool autoModeInProgress = false;
bool manualModeInProgress = false;
bool forceManual = false;
bool serialOut = true;
bool macroMode = false;
bool macroModeInProgress = false;
bool forward = false;
bool reverse = false;
bool left = false;
bool right = false;
bool climb = false;
bool dive = false;
bool rLeft = false;
bool rRight = false;
bool controlByteChanged = false;
bool controlByteBad = false;
bool ledOn = false;
bool heartBeatChecked = false;
unsigned char controlByte;
unsigned char lastControlByte;
unsigned char temp;
char *r;


//NEW block method of sending data
#define STARTBLOCK 204
#define STOPBLOCK 190
#define RESETBLOCK 195
bool blockStarted = false;
int block[10];
int blockCounter = 0;
bool blockCompleted = false;
bool blockBlock = false;
int blockSkipped = 0 ;

//Head control variables
long headCurrentPWM = STOP;
int headSpeedFactor = 100;
int headDirection = 1;
long headLastUpdate = millis();
long headStopPointPWM = MAXPWM;
int headStopPoint = 0;
int headDelay = 5000;
int headDivFactor = 1;
//A function that randomly moves the head around

void MoveHead()
{
  
    
 //SoftPWMServoServoWrite

        if(millis() - headLastUpdate > headDelay)      
 {
  
    if(headDirection == 0)
   headCurrentPWM -= headSpeedFactor;
  else
   headCurrentPWM += headSpeedFactor;

  if(random(1,10000) < 30)
                {
                  headDirection++;
                  headDirection %= 2;
                  headDelay=random(1,3) * 1000;
                  headSpeedFactor = random(1,6);
                  headSpeedFactor =10;
                }
                else  
                  headDelay = 10;
                  
                if(headCurrentPWM >= MAXPWM || headCurrentPWM <= MINPWM)
  {
                    
   if(headDirection == 0)
                        {
                            headDirection = 1;
                            headCurrentPWM = MINPWM + 10;
                        }
                        else
                        {
                            headDirection = 0;
                            headCurrentPWM = MAXPWM - 10;
                        }
                        
   headSpeedFactor = random(1,3) * 10;
;
                        
                        //headCurrentPWM = headStopPointPWM;
                        
                     
                           //Wait up to 3 seconds before moving again
                        headDelay = random(1,3);
                        headDelay *= 1000;
  
  }
                else
                {
                 
                        SoftPWMServoServoWrite(HEADSERVO,headCurrentPWM);
              headLastUpdate = millis();
                }
 
 }
 
}

bool ControlByteCheck(unsigned char cb,unsigned char cbc)
{
 bool valid = true;
 ////////Serial1.print("CB: ");
 ////////Serial1.println(cb);
 ////////Serial1.print("CBC: ");
 ////////Serial1.println(cbc);
if(cb % 17 != cbc)
  valid =  false;
//Check for opposing motions, which may mean there is an issue
 //forward and reverse requested
 if(cb % 2 && (cb>>1) % 2)
  valid = false;
 //left and right requested
 if( (cb>>2) % 2 && (cb>>3) % 2)
  valid = false; 
 //rotate left and rotate right requested
 if((cb>>4) % 2 && (cb>>5) % 2)
  valid = false;
 //climb and dive requested
 if((cb>>6) %2 && (cb>>7) %2)
  valid = false;
return valid; 
 
 
 
}
inline int ParseControlByte()
{
 int cb = controlByte;
 rRight = false;
 rLeft = false;
 forward = false;
 reverse = false;
 left = false;
 right = false;
 climb = false;
 dive = false;

  if(cb & 1)
                {
   rRight = true;
                        rSpeed = ROTATERIGHT;
                }
  cb >>= 1;
                if(cb & 1)
                {
                        rLeft = true;
                        rSpeed = ROTATELEFT;
                }
                cb >>= 1;
                if(cb & 1)
                {
                        dive = true;
                        zSpeed = MOVEDIVE;
                }
                cb >>= 1;
                if(cb & 1)
                {
                        climb = true;
                        zSpeed = MOVECLIMB;
                }
                cb >>= 1;
                if(cb & 1)
                {
                        right = true;
                        xSpeed = MOVERIGHT;
                }
                cb >>= 1;
                if(cb & 1)
                {
                        left = true;
                        xSpeed = MOVELEFT;
                }
                cb >>= 1;
                if(cb & 1)
                {
                        reverse = true;
                        ySpeed = MOVEREVERSE;
                }
                cb >>= 1;
                if(cb & 1)
                {
                        forward = true;
                        ySpeed = MOVEFORWARD;
                }
                
                //Set default speeds here
                if(!forward && !reverse)
                    ySpeed = STOP;
                if(!left && !right)
                    xSpeed = STOP;
                if(!climb && !dive)
                    zSpeed = STOPHOVER;

}
void PrintDirections()
{
 ////Serial1.println("------------------------------");
//if(forward)
  ////Serial1.println("FORWARD");
 //if(reverse)
  //////Serial1.println("REVERSE");
 //if(left)
  //////Serial1.println("LEFT");
 //if(right)
  //////Serial1.println("RIGHT");
 //if(climb)
  //////Serial1.println("CLIMB");
 //if(dive)
  //////Serial1.println("DIVE");
 //if(rLeft)
  //////Serial1.println("RLEFT");
 //if(rRight)
  //////Serial1.println("RRIGHT");
        //////Serial1.println("------------------------------");


}


void I2CReceiveEventBlock(int numBytes)
{
        //Every 2 bytes are our data pairs, writes come in groups of 3
        unsigned char cb, cbc, reg;
 cb = Wire.receive();
 if(!blockBlock)
 {
  
                //Block Start
                if(cb == 204)
  {
   blockStarted  = true;
   blockCounter = 0;
   return;
  }
                //Block End
  if(cb == 190)
  {
   if(blockStarted)
   {
    blockStarted = false;
    blockCompleted = true;
    blockBlock = true;
    return;
   }
  }
                //Block Reset
                if(cb == 195)
                {
                   blockStarted = false;
                   blockCompleted = false;
                   blockCounter = 0;
                }
 
                //Data
  if(blockStarted)
  {
   block[blockCounter++] = cb;
   blockCounter %= 10;
  }
 }
        else
          blockSkipped++;
}

void ProcessBlock()
{
  int reg = block[0];
  if(reg == 22)
  {
    //heartbeat check
    heartBeatChecked = true;
  }
  else if(reg == 60)
  {
    if(ControlByteCheck(block[1],block[2]))
    {
      lastControlByte = controlByte;
      controlByte = block[1];
      controlByteChanged = true;
      controlByteBad = false;
    }
    else if(reg == 90)
    {
        //Speed Change
        //block[1] is the direction and block[2] is to indicate speedup or slowdown
        int direction = block[1];
        int adjust = 0;
        
        if(block[2] == ADJUSTFASTER)
          adjust = FASTER;
        if(block[2] == ADJUSTSLOWER)
          adjust = SLOWER;
        
        
        if(direction == FORWARDADJUST || direction == REVERSEADJUST)
          ySpeed += adjust;
        if(direction == LEFTADJUST || direction == RIGHTADJUST)
          xSpeed += adjust;
          
        if(direction == RRIGHTADJUST || direction == RLEFTADJUST)
          rSpeed += adjust;
          
        if(direction == CLIMBADJUST || direction == DIVEADJUST)
            zSpeed += adjust;
            
        controlByteChanged = true;
        controlByteBad = false;
        
          
          
          
    }
      controlByteBad = true;
  }
  else
    controlByteBad = true;
  blockBlock = false;
  
}


void setup()
{
 r = new char[20];
 
 //Random numbers for Head movement.
 randomSeed(analogRead(A1));
//if(serialOut)
  ////Serial1.begin(9600);
 ////Serial1.println("RESET");
 //Setup RPi input pins
//Setup PWM pins going to flight controller

 //Setup PWM input pins from RX
 pinMode(RXCHANNEL1,INPUT);
 pinMode(RXCHANNEL2,INPUT);
  pinMode(RXCHANNEL3,INPUT);
  pinMode(RXCHANNEL4,INPUT);
  pinMode(RXCHANNEL6,INPUT);
 pinMode(RXCHANNEL5,INPUT);
pinMode(RPIAUTOMODE,OUTPUT);
 pinMode(RPIMACROMODE,OUTPUT);
digitalWrite(RPIAUTOMODE,LOW);
 digitalWrite(RPIMACROMODE,LOW);
Wire.begin(40);
        Wire.onReceive(I2CReceiveEventBlock);
//Center all the servos
 SoftPWMServoServoWrite(HEADSERVO,STOP);
        SoftPWMServoServoWrite(FLIGHTCONTROL_X, STOP);
 SoftPWMServoServoWrite(FLIGHTCONTROL_Y, STOP);
        SoftPWMServoServoWrite(FLIGHTCONTROL_Z, MINPWM);
        SoftPWMServoServoWrite(FLIGHTCONTROL_R, STOP);
 
}

inline int ReadPWM2(int pin)
{
 unsigned long m1,m2;
 int d = 0;
 unsigned long functionStart = millis();
 //for(int i=0;i<2 && millis()-functionStart <= READPWMMAXDELAY;i++)
 //{ 
 //wait for the pin to go low
 while(digitalRead(pin) == HIGH && millis()-functionStart <= READPWMMAXDELAYLOW);
 while(digitalRead(pin) == LOW && millis()-functionStart <= READPWMMAXDELAYLOW);
 m1 = micros();
 while(digitalRead(pin) == HIGH && millis()-functionStart <= READPWMMAXDELAYLOW);
 m2 = micros();
 d = (m2-m1);
 //}
 
 //d /= 2;

 ////////Serial1.print("pin: ");
 ////////Serial1.println(pin);
//if(d < MINPWM || d > MAXPWM)
 //f d = 0;
if(millis() - functionStart <= READPWMMAXDELAYLOW)
 {
  ////////Serial1.println(d);
  return d;
 }
 else
 {
  ////////Serial1.println(0);
  return 0;
 }
}

char * CToS(unsigned char c)
{
  
  char temp[20]; 
  char t;
   int i = 0;
   
  while(c > 0)
 {
     t = c % 10;
     c = c  / 10;
     t = t + '0';
     temp[i] = t;
     i++;
 } 
 int q = 0;
 for(int j=i-1;j>=0;j--)
   r[q++] = temp[j];
 r[i] = 0;
return r;  
}
//More Global Vars for loop
int channel1;
int prevChannel1 = 0;
int channel2;
int prevChannel2 = 0;
int channel3;
int prevChannel3 = 0;
int channel4;
int prevChannel4 = 0;
int channel6;
int channel5;

unsigned int channel1Errors = 0;
unsigned int channel2Errors = 0;
unsigned int channel3Errors = 0;
unsigned int channel4Errors = 0;
unsigned int channel5Errors = 0;
unsigned int channel6Errors = 0;
bool servo1Removed = false;
char controlChar = 0;


void loop()
{
      ////Serial1.println("HERE");
 unsigned int a = 0;
        int channel6LastChecked = 0;
        
                             
        
 
 while(1)
 {
          ///////Serial1.println("Loop");
          //delay(100);
         // autoMode = false;
       //Move the Head.
 MoveHead();

 if(!autoMode || forceManual)
 {
  if(!manualModeInProgress)
  {
   //////Serial1.println("Entering Manual Mode");
   manualModeInProgress = true;
   autoModeInProgress = false;
  }
/*
                if(heartBeatChecked)
                {
                    //////Serial1.println("Heartbeat Checked");
                    heartBeatChecked = false;
                }
                if(controlByteChanged)
                {
                  //////Serial1.print("Bytes REceived: ");
                  //////Serial1.println(CToS(temp));
                  controlByteChanged = false;
                }
                */
  //Main manual mode logic
  if(channel1Errors < 50)
   channel1 = ReadPWM2(RXCHANNEL1);
  if(channel1 == 0)
   ;//channel1Errorsx++;
  else
   channel1Errors = 0;
 

  if(channel1Errors < 50 && channel1 != 0 && abs(prevChannel1 - channel1) > SERVODEADBAND)
  {
   //Serial1.print(prevChannel1); 
   //Serial1.print("---");
   //Serial1.println(channel1);
   //Serial1.println(abs(prevChannel1 - channel1));

   prevChannel1 = channel1;
   //velocityX.writeMicroseconds(channel1);
                        SoftPWMServoServoWrite(FLIGHTCONTROL_X, channel1);
                        servo1Removed = false;
  }
                else
                {
                  if(channel1Errors > 50)
                  {
                    if(!servo1Removed)
                    {
                        //Serial1.println("SERVO 1 REMOVED"); 
                        servo1Removed = true;
                    } 
                    else
                    {
                      channel1Errors++;
                      channel1Errors %= 5000;  //After 500 cycles see if the servo comes back up.
                    }
                  }
                }
  if(channel2Errors < 50) 
   channel2 = ReadPWM2(RXCHANNEL2);
  if(channel2 == 0) 
   ;//channel2Errors++;
  else
   channel2Errors = 0;
 
  if(channel2 != 0 && abs(prevChannel2 - channel2) > SERVODEADBAND)
  {
   SoftPWMServoServoWrite(FLIGHTCONTROL_Y, channel2);
   prevChannel2 = channel2;
  }
  if(channel3Errors < 50) 
   channel3 = ReadPWM2(RXCHANNEL3);
  if(channel3 == 0)
   ;//channel3Errors++;
  else
   channel3Errors = 0;
  if(channel3 != 0 && abs(prevChannel3 - channel3) > SERVODEADBAND)
  {
   SoftPWMServoServoWrite(FLIGHTCONTROL_Z, channel3);
   prevChannel3 = channel3;
  
  }
  if(channel4Errors < 50) 
   channel4 = ReadPWM2(RXCHANNEL4); 
  if(channel4 == 0)
   ;//channel4Errors++;
  else
   channel4Errors = 0;
  if(channel4 != 0 && abs(prevChannel4 - channel4) > SERVODEADBAND)
  {
   SoftPWMServoServoWrite(FLIGHTCONTROL_R, channel4);
   prevChannel4 = channel4;
  }

  channel6 = ReadPWM2(RXCHANNEL6);
  
  if(channel6 == 0 || channel6 >= STOP)
  {
   autoMode = true;
   digitalWrite(RPIAUTOMODE,HIGH);
  }
  else
  {
   if(channel5Errors < 50)
    channel5 = ReadPWM2(RXCHANNEL5);
   if(channel5 == 0)
   channel5Errors++;
   else
    channel5Errors = 0;
   if(channel5 > STOP)
   {
    macroMode = true;
    if(!macroModeInProgress)
    {
     digitalWrite(RPIMACROMODE,HIGH);
     //////Serial1.println("Entering Macro Record Mode.");
     macroModeInProgress = true;
    }
   }
   else
   {
    if(macroModeInProgress)
    {
     macroModeInProgress = false;
     macroMode = false;
     //////Serial1.println("Leaving Macro Record Mode.");
     digitalWrite(RPIMACROMODE,LOW); 
    }
   }
  }
 }
 else
 {
  //Main automode logic here
  if(!autoModeInProgress)
  {
   //////Serial1.println("Entering AUTOMODE");
   autoModeInProgress = true;
   manualModeInProgress =false;
   macroMode = false;
   digitalWrite(RPIMACROMODE,LOW);
   digitalWrite(RPIAUTOMODE,HIGH);
  }
  //automode get directions from Rpi
                //Do Testing
                
                         
                if(blockBlock &&   blockCompleted)
                {
                  ////////Serial1.println("BLOCK");
                  ProcessBlock();
                  //if(controlByteBad)
                    //////Serial1.println("BAD");
                  //else
                    //////Serial1.println("GOOD");
                  ////////Serial1.println(CToS(block[0]));
                  ////////Serial1.println(CToS(block[1]));
                  ////////Serial1.println(CToS(block[2]));
                  ////////Serial1.print("Skipped: ");
                  //////Serial1.println(blockSkipped);
                       
                }
  
  if(controlByteChanged)
  {
   controlByteChanged = false;
   ParseControlByte();
   //PrintDirections();

                            SoftPWMServoServoWrite(FLIGHTCONTROL_X, xSpeed);
                            SoftPWMServoServoWrite(FLIGHTCONTROL_Y, ySpeed);
                            SoftPWMServoServoWrite(FLIGHTCONTROL_Z, zSpeed);
                            SoftPWMServoServoWrite(FLIGHTCONTROL_R, rSpeed);

  }
                if(millis() - channel6LastChecked > 2000)
                {
          channel6 = ReadPWM2(RXCHANNEL6);
          if(channel6 != 0 &&  channel6 < STOP)
          {
              ////////Serial1.println(channel6); 
                     autoMode = false;
       digitalWrite(RPIAUTOMODE,LOW);
          }
                        channel6LastChecked =millis();
                }


}
 }
}