Tuesday, December 22, 2015

Current Sensing Feedback Motor Control PcDuino 4wD Arduino Motor Sheild







This update changes the motor control to regulate current on both sides of the motors. Two motors are connected, per channel, in parallel. The Motor Shield can only control two dc motors so the 4wd motors are wired together per side.


TimerInterupt


This Algorithm has a Timer Interrupt. Triggering a an function that reads motor pins A0 and A1 for the current and implements a feedback loop.

void timer_handler(int irq){

switch(irq){

  case SIGALRM:

         average_currentA=analogRead(currentPinA);
        average_currentB=analogRead(currentPinB);
      //  average_currentA/=2;
      //  average_currentB/=2; //digitalWrite(13,(digitalRead(13)^0×01));
        int diffa=abs(setspeedA)-average_currentA;
        int diffb=abs(setspeedB)-average_currentB;
      // printf("%i,%i %i,%i\n",average_currentA,average_currentB,diffa,diffb);
        curspeedA+=diffa*0.1;
        curspeedB+=diffb*0.1;
       if(curspeedA<0)curspeedA=0;
       if(curspeedB<0)curspeedB=0;
       
       
       
       if(setspeedA>0)
         digitalWrite(dirA,HIGH);
        else digitalWrite(dirA,LOW);
       if(setspeedB<0)
         digitalWrite(dirB,HIGH);
        else digitalWrite(dirB,LOW);
        
         
         analogWrite(speedA,int(abs(curspeedA)));
         analogWrite(speedB,int(abs(curspeedB)));

        break;

  

  };

}

Update to the rover.c file:




#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <errno.h>
#include <string.h>
#include <sys/types.h>
#include <time.h> 

#include <core.h>




int dirA = 12;
int dirB = 13;  // not used in this example
int speedA = 3;
int speedB = 11; // not used in this example
int currentPinA = A0;
int currentPinB = A1;

int average_currentA,average_currentB;
int setspeedA,setspeedB;
float curspeedA,curspeedB;

int listenfd = 0, connfd = 0;
int n;
struct sockaddr_in serv_addr; 

 char sendBuff[1025];
  time_t ticks; 
unsigned int counter;


void timer_handler(int irq);

//////////////////////////////////////////////////////////////////////////////////////
#define REFRESH_TIME_S 0

#define REFRESH_TIME_US 5000

void time_initialization(long s,long us)

{

struct itimerval interrupt_time;

interrupt_time.it_value.tv_sec = s;

interrupt_time.it_value.tv_usec =us;

interrupt_time.it_interval.tv_sec = s;

interrupt_time.it_interval.tv_usec = us;

setitimer(ITIMER_REAL, &interrupt_time, NULL);

signal(SIGALRM, timer_handler);

printf("set interrupt time: %lds , %ldus\n", s, us);

}


/**
   * readline() - read an entire line from a file descriptor until a newline.
    * functions returns the number of characters read but not including the
  * null character.
  **/
  int readline(int fd, char *str, int maxlen) 
{
  int n;           /* no. of chars */  
    int readcount;   /* no. characters read */
   char c;

  for (n = 1; n < maxlen; n++) {
        /* read 1 character at a time */
     readcount = read(fd, &c, 1); /* store result in readcount */
    if (readcount == 1) /* 1 char read? */
     {
      *str = c;      /* copy character to buffer */
        str++;         /* increment buffer index */         
      if (c == '\n') /* is it a newline character? */
            break;      /* then exit for loop */
        } 
    //else if (readcount == 0) /* no character read? */
 //     {
 //      if (n == 1)   /* no character read? */
  //        return (0); /* then return 0 */
 //     else
//         break;      /* else simply exit loop */
   //  } 
    else 
       return (-1); /* error in read() */
   }
   *str=0;       /* null-terminate the buffer */
    return (n);   /* return number of characters read */
} /* readline() */

void setup()
{
 counter=0;
  
pinMode (dirA, OUTPUT);
  pinMode (dirB, OUTPUT);
  pinMode (speedA, OUTPUT);
  pinMode (speedB, OUTPUT);

digitalWrite(speedA,LOW);
digitalWrite(speedB,LOW);
   listenfd = socket(AF_INET, SOCK_STREAM, 0);
    memset(&serv_addr, '0', sizeof(serv_addr));
    memset(sendBuff, '0', sizeof(sendBuff)); 

    serv_addr.sin_family = AF_INET;
    serv_addr.sin_addr.s_addr = htonl(INADDR_ANY);
    serv_addr.sin_port = htons(5000); 

    bind(listenfd, (struct sockaddr*)&serv_addr, sizeof(serv_addr)); 

    listen(listenfd, 10); 

    connfd = accept(listenfd, (struct sockaddr*)NULL, NULL); 
     printf("connected\n");  
       curspeedA=0;curspeedB=0;
     setspeedA=0;setspeedB=0;
     average_currentA=0;
     average_currentB=0;
     time_initialization(REFRESH_TIME_S,REFRESH_TIME_US);
}
///////////////////////////////////////////////////////////////////////////////////////////////////



void timer_handler(int irq){

switch(irq){

  case SIGALRM:

         average_currentA=analogRead(currentPinA);
        average_currentB=analogRead(currentPinB);
      //  average_currentA/=2;
      //  average_currentB/=2; //digitalWrite(13,(digitalRead(13)^0×01));
        int diffa=abs(setspeedA)-average_currentA;
        int diffb=abs(setspeedB)-average_currentB;
      // printf("%i,%i %i,%i\n",average_currentA,average_currentB,diffa,diffb);
        curspeedA+=diffa*0.1;
        curspeedB+=diffb*0.1;
       if(curspeedA<0)curspeedA=0;
       if(curspeedB<0)curspeedB=0;
       
       
       
       if(setspeedA>0)
         digitalWrite(dirA,HIGH);
        else digitalWrite(dirA,LOW);
       if(setspeedB<0)
         digitalWrite(dirB,HIGH);
        else digitalWrite(dirB,LOW);
        
         
         analogWrite(speedA,int(abs(curspeedA)));
         analogWrite(speedB,int(abs(curspeedB)));

        break;

  

  };

}

/////////////////////////////////////////////////////////////////////////////////////////////////

void loop()
{




    
    
      n=readline(connfd,sendBuff,8);
      // n = read(connfd, sendBuff, strlen(sendBuff) ); 
      printf("%d:",n);
     if(n>0)
    {
   printf("%s",sendBuff);
 
  if(sendBuff[0]=='B'){
 digitalWrite(speedA,LOW);
 digitalWrite(speedB,LOW);
 
  }
  else if(sendBuff[0]=='C'){
           char *p=sendBuff;p++;
     int k=strtol(p,&p,10);
     if(k<0){
       digitalWrite(dirA,HIGH);
           digitalWrite(dirB,HIGH);
           }
     else {
              digitalWrite(dirA,LOW);
     digitalWrite(dirB,LOW);
     }
       setspeedA=(k)*10;
       setspeedB=(k)*10;
       
      //  analogWrite(speedA,abs(k)*10);
  //analogWrite(speedB,abs(k)*10);
  }
  
  
  else if(sendBuff[0]=='A')
  {
     char *p=sendBuff; 
  p++;
   int j=strtol(p,&p,10); 
 printf("%d\n",j);
  
  
  
  setspeedA=abs(j);
  setspeedB=abs(j);
  
 //analogWrite(speedB,abs(j));
  //  analogWrite (speedA,abs(j));
// delay(100);
 }
  else if(sendBuff[0]=='E'){
  // snprintf(sendBuff, sizeof(sendBuff), "%i,%i\n",analogRead(currentPinA),analogRead(currentPinB));
   // write(connfd, sendBuff, strlen(sendBuff)); 
    //printf("%s",sendBuff);
    
  
    }
    }
    else { //reconnect
    
    setspeedA=0;setspeedB=0;
      digitalWrite(speedA,LOW);
 digitalWrite(speedB,LOW);
    
       connfd = accept(listenfd, (struct sockaddr*)NULL, NULL); 
     printf("connected\n");  
      
      
    }

 
  //    close(connfd);
  //   sleep(1);


}

Saturday, December 19, 2015

Streaming Camera PCDuino 4WD Platform

Microsoft Life Cam streaming on a pcduino



video




Streaming from the PCDuino.


First Install VLC

sudo apt-get install vlc


The following commands use v4l2 interface and are transcoded to a real time streaming protocol.
Here are four commands for vlc.

cvlc v4l2:// :v4l2-vdev="/dev/video0" --sout '#transcode{vcodec=xvid,vb=400,width=240,heigh=180,acodec=mp4a,ab=32 ,channels=2,samplerate=22100}:duplicate{dst=std{access=http{mime=video/x-ms-wmv},mux=asf,dst=:8082/stream.wmv}}' --no-sout-audio



cvlc v4l2:// :v4l2-vdev="/dev/video0" --sout="#transcode{vcodec=xvid,vb=800,width=240,height=180}:rtp{mux=ts,dst=192.168.0.11,port=5004,sdp=rtsp://192.168.0.11:8080/test.sdp}}"



cvlc v4l2:// :v4l2-vdev="/dev/video0" :v4l2-width=160 :v4l2-height=120  --no-sout-audio --sout '#transcode{vcodec=xvid,vb=200,fps=1,width=160,height=120}:rtp{proto=udp,mux=ts,sdp=rtsp://192.168.0.11:4444/test.sdp}' --loop --ttl 1


This is the LOW latency command:


cvlc -vvv v4l2:///dev/video0:chroma=mjpg:width=160:height=120:fps=5  --live-caching=10 --no-audio --sout '#transcode{vcodec=yuv2,acodec=none,vb=0,width=160,height=120}:rtp{access=udp,mux=ts,url=192.168.0.11,sdp=rtsp://192.168.0.11:4444/test.sdp}'  --ttl 1





The Java Controller



The following image contains a putty.exe console to the rover. The Java Controller and VLC receiving the stream video.



The java source code for the Controllers.




import java.awt.*;
import java.awt.event.*;
import javax.swing.*;
import javax.swing.event.*;
import java.util.Hashtable;
import java.io.*;
import java.net.*;


/*
 * Copyright (c) 1995, 2008, Oracle and/or its affiliates. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 *   - Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *
 *   - Redistributions in binary form must reproduce the above copyright
 *     notice, this list of conditions and the following disclaimer in the
 *     documentation and/or other materials provided with the distribution.
 *
 *   - Neither the name of Oracle or the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
 * IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */ 



/*
 * TCPClient.java requires all the files in the images/doggy
 * directory.
 */
public class TCPClient extends JPanel
                         implements ActionListener,
                                    WindowListener,
                                    ChangeListener {
    //Set up animation parameters.
//    static final int FPS_MIN = 0;
//    static final int FPS_MAX = 30;
  //  static final int FPS_INIT = 15;    //initial frames per second
  //  int frameNumber = 0;
 //   int NUM_FRAMES = 14;
 //   ImageIcon[] images = new ImageIcon[NUM_FRAMES];
   // int delay;
    //Timer timer;
  //  boolean frozen = false;
     DataOutputStream outToServer;
        Socket clientSocket;
        int fps,fps4;
        JTextField textField;
        JButton connect,reverse;
        JSlider framesPerSecond,turn;
    //This label uses ImageIcon to show the doggy pictures.
 //   JLabel picture;

    public TCPClient() {

        super(new GridLayout(4,1));

        fps=0;fps4=0;
        clientSocket=null;
     

       //delay = 1000 / FPS_INIT;


       //text field
       textField = new JTextField("192.168.0.11",20);
       connect=new JButton("Connect");
       reverse=new JButton("Reverse");
      // textField.addActionListener(this);
       connect.addActionListener(this);
    reverse.addActionListener(this);
        //Create the slider.
        framesPerSecond = new JSlider(JSlider.VERTICAL,
                                              -5,5, 0);
        framesPerSecond.addChangeListener(this);
        framesPerSecond.setMajorTickSpacing(1);
        framesPerSecond.setPaintTicks(true);
        
        //Create the label table.
        Hashtable<Integer, JLabel> labelTable =  new Hashtable<Integer, JLabel>();
        //PENDING: could use images, but we don't have any good ones.
        labelTable.put(new Integer( 0 ),
                       new JLabel("Stop") );
                     //new JLabel(createImageIcon("images/stop.gif")) );
        labelTable.put(new Integer( 127 ),
                       new JLabel("Slow") );
                     //new JLabel(createImageIcon("images/slow.gif")) );
        labelTable.put(new Integer(255 ),
                       new JLabel("Fast") );
                     //new JLabel(createImageIcon("images/fast.gif")) );
        framesPerSecond.setLabelTable(labelTable);

        framesPerSecond.setPaintLabels(true);
        framesPerSecond.setBorder(
                BorderFactory.createEmptyBorder(0,0,0,10));

       

turn = new JSlider(JSlider.HORIZONTAL,-5,5, 0);
        turn.addChangeListener(this);
        turn.setMajorTickSpacing(1);
        turn.setPaintTicks(true);



        //Put everything together.
        add(framesPerSecond);
        add(turn);
        add(connect);
          add(reverse);
        add(textField);
        setBorder(BorderFactory.createEmptyBorder(10,10,10,10));

    }
    
    
    
    
    //////////////////////////////////////////////////////////////////////////////////////////////////

    /** Add a listener for window events. */
    void addWindowListener(Window w) {
        w.addWindowListener(this);
    }

    //React to window events.
    public void windowIconified(WindowEvent e) {
    //    stopAnimation();
    }
    public void windowDeiconified(WindowEvent e) {
      // startAnimation();
    }
    public void windowOpened(WindowEvent e) {}
    public void windowClosing(WindowEvent e) {    try
             {
               clientSocket.close();
             } 
             catch( IOException ioe) {}  }
    public void windowClosed(WindowEvent e) {}
    public void windowActivated(WindowEvent e) {}
    public void windowDeactivated(WindowEvent e) {}




/////////////////////////////////////////////////////////////////////////////////////////STATECHANGE///////////////////////////////////
    /** Listen to the slider. */
    public void stateChanged(ChangeEvent e)  {
        
        
          if(clientSocket!=null){
           if( e.getSource()==framesPerSecond){
      
        
             JSlider source = (JSlider)e.getSource();
             int fps2 = (int)source.getValue();
             
             
             if(fps!=fps2){
                 String outstring;
            //outstring=(char)fps;
                try
                {
                      outstring="A"+new String(Integer.toString(fps2*10));
                         outstring+="\n";
                  System.out.println(fps2);
                   outToServer.writeBytes(outstring);
                    
                } 
                catch( IOException ioe) {System.out.println("ERROR");}
                 fps=fps2;
            }
        }
         else if( e.getSource()==turn){
             
                
            if(fps==0){
                
                JSlider source = (JSlider)e.getSource();
             int fps3 = (int)source.getValue();
                 if(fps4!=fps3){
                 String outstring;
                try
                {
                 
                      outstring="C"+new String(Integer.toString(fps3));
                         outstring+="\n";
                  System.out.println(outstring);
                   outToServer.writeBytes(outstring);
                 
                   
                } 
                catch( IOException ioe) {System.out.println("ERROR");}
                fps4=fps3;
            }
            }
         }
            
        }
    }

   //////////////////////////////////////////////////////////////////////////////////////////ACTIONPERFORMED/////////////////////////////



    //Called when the Timer fires.
    public void actionPerformed(ActionEvent e) {
            if( e.getSource()==connect){
                try
                 {
                    if(clientSocket!=null)clientSocket.close();
                    clientSocket  = new Socket(textField.getText(), 5000);
                    outToServer = new DataOutputStream(clientSocket.getOutputStream());
                 }
                 catch(IOException ioe){ clientSocket=null;}
          }
         else   if( e.getSource()==reverse){
              try
                 {
                     
                     System.out.print("Reverse");
                   System.out.println(fps);
                   if(fps==0) {
                      System.out.println("Send");
                       outToServer.writeBytes("R\n"); 
                    }}
                 catch(IOException ioe){ clientSocket=null;}
            }
    
    
    }

    

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////    
    
    
    
    
    
    /**
     * Create the GUI and show it.  For thread safety,
     * this method should be invoked from the
     * event-dispatching thread.
     */
  

    public static void main(String[] args)  {
   
     
  
  //BufferedReader inFromServer = new BufferedReader(new InputStreamReader(clientSocket.getInputStream()));
  //sentence = inFromUser.readLine();
  //outToServer.writeBytes(sentence + '\n');
 // modifiedSentence = inFromServer.readLine();
  //System.out.println("FROM SERVER: " + modifiedSentence);

  
  
        /* Turn off metal's use of bold fonts */
        UIManager.put("swing.boldMetal", Boolean.FALSE);
        
        //Schedule a job for the event-dispatching thread:
       JFrame frame = new JFrame("SliderDemo");
        frame.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
        TCPClient animator = new TCPClient();
                
        //Add content to the window.
        frame.add(animator, BorderLayout.CENTER);

        //Display the window.
        frame.pack();
        frame.setVisible(true);
   
    
    }
}

The PCDuino Code for the Motor Control.

download this https://github.com/pcduino/c_environment to compile.

you can add this script to your startup by:

make the following file   '/etc/init/rover.conf'  with the following contents:


 start on started networking
 task
 exec /home/linaro/Arduino/c_environment-master/output/test/linker_led_test


Use these commands to start the rover:

sudo start rover


To Stop:

sudo stop rover

Status:

sudo status rover


///////////////////////////////////////////////////////////////////////////////////////////////////////////////////

C source code:


/*
* LED test program
* The LED will be controlled by TCP socket
* This program serves an example of TCP socket server
*The TCP socket code is adpoted from : http://www.thegeekstuff.com/2011/12/c-socket-programming/
* Please use linker kit LED module,and install it on D2 D3 V G postion
*
* Jingfeng Liu
*  5/4/2013
*
*/


#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <errno.h>
#include <string.h>
#include <sys/types.h>
#include <time.h> 

#include <core.h>




int dirA = 12;
int dirB = 13;  // not used in this example
int speedA = 3;
int speedB = 11; // not used in this example


int listenfd = 0, connfd = 0;
int n;
struct sockaddr_in serv_addr; 

 char sendBuff[1025];
  time_t ticks; 


/**
   * readline() - read an entire line from a file descriptor until a newline.
    * functions returns the number of characters read but not including the
* null character.
**/
  int readline(int fd, char *str, int maxlen) 
{
  int n;           /* no. of chars */  
    int readcount;   /* no. characters read */
 char c;

for (n = 1; n < maxlen; n++) {
   /* read 1 character at a time */
   readcount = read(fd, &c, 1); /* store result in readcount */
    if (readcount == 1) /* 1 char read? */
   {
      *str = c;      /* copy character to buffer */
        str++;         /* increment buffer index */         
      if (c == '\n') /* is it a newline character? */
          break;      /* then exit for loop */
     } 
    //else if (readcount == 0) /* no character read? */
//    {
//      if (n == 1)   /* no character read? */
  //        return (0); /* then return 0 */
 //     else
//        break;      /* else simply exit loop */
 //  } 
    else 
     return (-1); /* error in read() */
 }
 *str=0;       /* null-terminate the buffer */
    return (n);   /* return number of characters read */
} /* readline() */

void setup()
{
         
  
pinMode (dirA, OUTPUT);
  pinMode (dirB, OUTPUT);
  pinMode (speedA, OUTPUT);
  pinMode (speedB, OUTPUT);

digitalWrite(speedA,LOW);
digitalWrite(speedB,LOW);
   listenfd = socket(AF_INET, SOCK_STREAM, 0);
    memset(&serv_addr, '0', sizeof(serv_addr));
    memset(sendBuff, '0', sizeof(sendBuff)); 

    serv_addr.sin_family = AF_INET;
    serv_addr.sin_addr.s_addr = htonl(INADDR_ANY);
    serv_addr.sin_port = htons(5000); 

    bind(listenfd, (struct sockaddr*)&serv_addr, sizeof(serv_addr)); 

    listen(listenfd, 10); 

    connfd = accept(listenfd, (struct sockaddr*)NULL, NULL); 
     printf("connected\n");  
}

void loop()
{

//     ticks = time(NULL);
 //    snprintf(sendBuff, sizeof(sendBuff), "Jingfeng Liu%.24s\r\n", ctime(&ticks));
  //     write(connfd, sendBuff, strlen(sendBuff)); 
      n=readline(connfd,sendBuff,8);
      // n = read(connfd, sendBuff, strlen(sendBuff) ); 
      printf("%d:",n);
     if(n>0)
    {
printf("%s",sendBuff);
if(sendBuff[0]=='B'){
digitalWrite(speedA,LOW);
digitalWrite(speedB,LOW);
}
else if(sendBuff[0]=='C'){
           char *p=sendBuff;p++;
  int k=strtol(p,&p,10);
  if(k<0){
  digitalWrite(dirA,HIGH);
           digitalWrite(dirB,HIGH);
           }
  else {
              digitalWrite(dirA,LOW);
 digitalWrite(dirB,LOW);
  }
        analogWrite(speedA,abs(k)*10);
analogWrite(speedB,abs(k)*10);
}
else if(sendBuff[0]=='A')
{
     char *p=sendBuff;
p++;
   int j=strtol(p,&p,10); 
printf("%d\n",j);
  
  if(j<0){
digitalWrite (dirA, HIGH);
digitalWrite(dirB,LOW);
  }
  else {
    digitalWrite (dirA,LOW);
digitalWrite(dirB,HIGH);
  }
  
  
analogWrite(speedB,abs(j));
    analogWrite (speedA,abs(j));
// delay(100);
}
    }
    else { //reconnect
    
    
      digitalWrite(speedA,LOW);
digitalWrite(speedB,LOW);
    
       connfd = accept(listenfd, (struct sockaddr*)NULL, NULL); 
     printf("connected\n");  
      
      
    }

  //    close(connfd);
  //   sleep(1);


}