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);


}









Saturday, October 10, 2015

Pwd3r SLS Printer...PLAN B modification from ytec3d.com


If anyone is familar with Plan-B powder 3d printer.


http://ytec3d.com/plan-b/


This is an enclosure for the PLAN-B with some modifications.
Most of the Plan B Parts such as the hopper fit inside this.



Creative Commons Attribution Non-Commerical 2015



Monday, March 23, 2015

Vesta Dwarf Planet Win32 Console Code


To compile you will need this 500Mb file:
http://dawndata.igpp.ucla.edu/tw.jsp?section=geometry/ShapeModels/GASKELL_CLAUDIA_2013_02_13/VEST64_DTM.raw

You will need the GLUT32 libraries.
https://www.opengl.org/resources/libraries/glut/

 "agviewer" for details of this open source api.
https://www.opengl.org/archives/resources/code/samples/glut_examples/contrib/contrib.html




#include "agviewer.h"

typedef enum {NOTALLOWED, AXES, STUFF, RING } DisplayLists;

int DrawAxes = 0;

#define ROTATEINC 0.02;

GLfloat Rotation = 0;  /* start ring flat and not spinning */
int     Rotating = 0;








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

void init1(){
int w=23041;
int h=11521;

  int ht=h+15;
  int wt=w+15;
  float scale=200.0;
  float deltaPhi=(16.0*PI/ht);
  float deltaTheta=(32.0*PI/wt);
  
  


  int length=w*h;
int size=0;

char *memblock;
unsigned short *memblock2;

  ifstream myfile;
  myfile.open ("VEST64_DTM.raw",ios::in|ios::binary|ios::ate);
  if (myfile.is_open()){
  

    size = myfile.tellg();

if(size==2*length){
cout<<size<<"=="<<2*length<<endl;

  }

    memblock=new char[2*length];
    myfile.seekg (0, ios::beg);
myfile.read(memblock,size);
myfile.close();
  }
  


  
  //unsigned short *a=new unsigned short[w][h];



  memblock2=new unsigned short[length];

for(int i=0;i<length;i++){
memblock2[i]=(memblock[i*2]<<8)+(memblock[i*2+1]);

//cout<<setw(8)<<memblock2[i];

}

delete memblock;

 unsigned short** ary = new  unsigned short*[ht];
for(int i = 0; i < ht; ++i)
    ary[i] = new unsigned short[wt];

int m,n;
for(int i=0;i<length;i++){     
 m=i%w,n=i/w;
//  cout<<setw(8)<<memblock2[i];
   ary[n][m]=memblock2[i]; 
  }
  //fill
for(int k=0;k<h;k++){
for(int j=w;j<wt;j++){

ary[k][j]=0;
}
}
for(int k=h;k<ht;k++){
for(int j=0;j<wt;j++){
ary[k][j]=0;
}
}

delete memblock2;

  unsigned short** b_ary = new  unsigned short*[ht];
for(int i = 0; i < ht; ++i)
    b_ary[i] = new unsigned short[wt/16];

for(int k=0;k<(ht);k++){
  for(int j=0;j<wt;j+=16){
 b_ary[k][j/16]=0;
 for(int l=0;l<16;l++){
 b_ary[k][j/16]+=(ary[k][j+l])/16;
// cout<<setw(8)<<b_ary[k/2][j/2];
 }
 }
}
for(int i = 0; i < ht; ++i) {
    delete [] ary[i];
    }
delete [] ary;

unsigned short** e_ary = new unsigned short*[(ht)/16];
for(int i = 0; i < (ht)/16; ++i)
    e_ary[i] = new unsigned short[(wt)/16];

  for(int k=0;k<(ht);k+=16){
  for(int j=0;j<(wt)/16;j++){
 e_ary[k/16][j]=0;
 for(int l=0;l<16;l++){
 e_ary[k/16][j]+=b_ary[k+l][j]/16;
 }
  }
}

for(int i = 0; i < (ht); ++i) {
    delete [] b_ary[i];
    }
delete [] b_ary;





  int u=0,v=0;
  glBegin(GL_TRIANGLES);
for(float theta=0;theta<2.0*PI-2.0*deltaTheta;theta+=deltaTheta){
v=0;
 for(float phi=0;phi<(PI-2.0*deltaPhi);phi+=deltaPhi){
 
 float r1= (+250.00000 - 0.3807671317E+02 + (e_ary[v][u+1])*0.1234605163E-02)/scale;
 float r2= (+250.00000 - 0.3807671317E+02 + (e_ary[v][u])*0.1234605163E-02)/scale;
 float r3= (+250.00000 - 0.3807671317E+02 + (e_ary[v+1][u])*0.1234605163E-02)/scale;
 float r4= (+250.00000 - 0.3807671317E+02 + (e_ary[v+1][u+1])*0.1234605163E-02)/scale;

 float z2=r2*cos(phi);
 float x2=r2*sin(phi)*cos(theta);
 float y2=r2*sin(phi)*sin(theta);
    
 float z1=r1*cos(phi);
 float x1=r1*sin(phi)*cos(theta+deltaTheta);
 float y1=r1*sin(phi)*sin(theta+deltaTheta);

 float z3=r3*cos(phi+deltaPhi);
 float x3=r3*sin(phi+deltaPhi)*cos(theta);
 float y3=r3*sin(phi+deltaPhi)*sin(theta);

 float z4=r4*cos(phi+deltaPhi);
 float x4=r4*sin(phi+deltaPhi)*cos(theta+deltaTheta);
 float y4=r4*sin(phi+deltaPhi)*sin(theta+deltaTheta);


  float V1x=x4-x1;
  float V1y=y4-y1;
  float V1z=z4-z1;

  float V2x=x2-x1;
  float V2y=y2-y1;
  float V2z=z2-z1;

  float normalx = V1y * V2z - V1z * V2y;
  float normaly = V2x * V1z - V2z * V1x;
  float normalz = V1x * V2y - V1y * V2x;

  float V3x=x3-x2;
  float V3y=y3-y2;
  float V3z=z3-z2;

  float V4x=x3-x4;
  float V4y=y3-y4;
  float V4z=z3-z4;

  float normalx2 = V3y * V4z - V3z * V4y;
  float normaly2 = V4x * V3z - V4z * V3x;
  float normalz2 = V3x * V4y - V3y * V4x;
  
 glNormal3f(normalx,normaly,normalz);
 
   glVertex3f( x1, y1, z1);  
  glVertex3f( x2, y2, z2);
   glVertex3f( x4, y4, z4);
  glNormal3f(normalx2,normaly2,normalz2);
 glVertex3f( x3, y3, z3);
glVertex3f( x4, y4, z4);
glVertex3f( x2, y2, z2);
 

  

 
v++;
 }
 u++;
}
glEnd();


for(int i = 0; i < ht/16; ++i) {
    delete [] e_ary[i];
    }
delete [] e_ary;


// for(int i = 0; i < h/8; ++i) {
  //  delete [] d_ary[i];
    //}
//delete [] d_ary;





//     return 0;




}








void myGLInit(void)
{

  GLfloat mat_ambuse[] = { 0.6, 0.0, 0.0, 1.0 };
  GLfloat mat_specular[] = { 0.4, 0.4, 0.4, 1.0 };

  GLfloat light0_position[] = { 0.6, 0.4, 0.3, 0.0 };

  glLightfv(GL_LIGHT0, GL_POSITION, light0_position);
  glEnable(GL_LIGHTING);
  glEnable(GL_LIGHT0);

  glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, mat_ambuse);
  glMaterialfv(GL_FRONT, GL_SPECULAR, mat_specular);
  glMaterialf(GL_FRONT, GL_SHININESS, 25.0);
  
  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();
  glMatrixMode(GL_MODELVIEW);
  glLoadIdentity();

  glEnable(GL_NORMALIZE);

  glDepthFunc(GL_LESS);
  glEnable(GL_DEPTH_TEST);

  glShadeModel(GL_SMOOTH);

  glFlush();



void MakeDisplayLists(void)
{
  glNewList(STUFF, GL_COMPILE);
  glPushMatrix();
    glutSolidCube(1.0);
    glTranslatef(2, 0, 0);
    glutSolidSphere(0.5, 10, 10);
    glTranslatef(-2, 0, 3);
    glRotatef(-90, 1, 0, 0);
    glutSolidCone(0.5, 1.0, 8, 8);
  glPopMatrix();
  glEndList();

  glNewList(RING, GL_COMPILE);
    init1();
  glEndList();
}

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void display(void)
{
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();

  gluPerspective(60, 1, 0.01, 100);

    /* so this replaces gluLookAt or equiv */
  agvViewTransform();

  glMatrixMode(GL_MODELVIEW);
  glLoadIdentity();

    /* we call agvMakeAxesList() to make this display list */
  if (DrawAxes)
    glCallList(AXES);

 // glCallList(STUFF);

 // glTranslatef(-2, 1, -2);
  glRotatef(Rotation, 1, 0, 0);
  glCallList(RING);

  glutSwapBuffers();
  glFlush();
}

//////////////////////////////////////////////////////////////////////////////////////////////////
  /* rotate the axis and adjust position if nec. */
void rotatethering(void)

  Rotation += ROTATEINC;

  if (agvMoving)   /* we since we are the only idle function, we must */
    agvMove();     /* give AGV the chance to update the eye position */

  glutPostRedisplay();
}

typedef enum { MENU_AXES, MENU_QUIT, MENU_RING } MenuChoices;

void handlemenu(int value)
{
  switch (value) {
    case MENU_AXES:
      DrawAxes = !DrawAxes;
      break;
    case MENU_QUIT:
      exit(0);
      break;
    case MENU_RING:
      Rotating = !Rotating;
      if (Rotating) {
glutIdleFunc(rotatethering);    /* install our idle function */
agvSetAllowIdle(0);             /* and tell AGV to not */
      } else {
glutIdleFunc(NULL);    /* uninstall our idle function      */
agvSetAllowIdle(1);    /* and tell AGV it can mess with it */
      }
      break;
    }
  glutPostRedisplay();
}

void visible(int v)
{
  if (v == GLUT_VISIBLE) {
    if (Rotating) {
      glutIdleFunc(rotatethering);
      agvSetAllowIdle(0);
    } else {
      glutIdleFunc(NULL);
      agvSetAllowIdle(1);      
    }
  } else {
      glutIdleFunc(NULL);
      agvSetAllowIdle(0);
  }
}


void MenuInit(void)
{
  int sub2 = glutCreateMenu(agvSwitchMoveMode);   /* pass these right to */
  glutAddMenuEntry("Flying move",  FLYING);       /* agvSwitchMoveMode() */
  glutAddMenuEntry("Polar move",   POLAR);

  glutCreateMenu(handlemenu);
  glutAddSubMenu("Movement", sub2);
  glutAddMenuEntry("Toggle Axes", MENU_AXES);
  glutAddMenuEntry("Toggle ring rotation", MENU_RING);
  glutAddMenuEntry("Quit", MENU_QUIT);
  glutAttachMenu(GLUT_RIGHT_BUTTON);
}

//////////////////////////////////////////////////////////////////////////////////////////////////
int main(int argc, char** argv)
{
  glutInit(&argc, argv);
  glutInitWindowSize(512, 512);
  glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB);
  glutCreateWindow("AGV example");
  
  glutVisibilityFunc(visible);

  if (Rotating)
    glutIdleFunc(rotatethering);

    /*
     * let AGV know if it can mess with the idle function (if we've
     * just installed an idle function, we tell AGV it can't touch it)
     */

  agvInit(!Rotating);
    
    /* 
     * agvInit() installs mouse, motion, and keyboard handles, but 
     * we don't care for this example cause we only use right button menu
     */

  agvMakeAxesList(AXES);  /* create AGV axes */

  myGLInit(); 
  MakeDisplayLists();
  MenuInit();

  glutDisplayFunc(display);

  glutMainLoop();
  return 0;             /* ANSI C requires main to return int. */
}