API tools faq
paste
Advertisement
rharder

Motion Platform Arduino Controller

rharder
Sep 7th, 2011
154
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Java 13.62 KB | None | 0 0
raw download clone embed print report
  1. // $Id: MotionPlatformArduinoBoard.pde 507 2010-04-05 21:51:41Z rharder $
  2.  
  3. /*
  4. This is the control software for the motion platform. The software
  5.  is loaded onto an Arduino board with digital-to-audio converters (DAC)
  6.  to convert signals to the +/- voltage that the motion platform requires.
  7.  
  8.  Authors:
  9.  Robert Harder, 2010
  10.  */
  11.  
  12. // SPI and DAC Pins
  13. // It seems that Arduino requires that the
  14. // SPI pins (except slave select) be these
  15. // particular pins.
  16. // The DAC_CLEAR pin is to reset the AD7849
  17. // DAC chip that's part of the circuit.
  18. #define SPI_DATA_OUT 11
  19. #define SPI_DATA_IN  12
  20. #define SPI_CLOCK    13
  21. #define SPI_SLAVE_SELECT_1 10
  22. #define DAC_CLEAR_PIN 8
  23. unsigned int ROLL_PIN = 10;
  24. unsigned int PITCH_PIN = 9;
  25.  
  26. // Serial communication back to a user's computer
  27. #define BAUD 9600
  28. #define SERIAL_BUFFER_SIZE 20
  29. char serialBuffer[SERIAL_BUFFER_SIZE+1]; // Ensure buffer +1 always has null
  30. unsigned int serialBufferPosition = 0;
  31.  
  32.  
  33. // Motion Smoothing voltages
  34. // When roll, pitch, etc are set, the values
  35. // are adjusted smoothly rather than jumping right
  36. // to the new voltage. This is to reduce the "violence"
  37. // of the motion platform.
  38. #define MOTION_DIMENSIONS 2
  39. float curr[MOTION_DIMENSIONS];
  40. float goal[MOTION_DIMENSIONS];
  41. float start[MOTION_DIMENSIONS];
  42. // Constants used in a few places, especially array indeces
  43. #define ROLL 0
  44. #define PITCH 1
  45.  
  46. // Motion Smoothing, transitions
  47. // These values are used as part of the motion smoothing
  48. // procedure.
  49. float vel[MOTION_DIMENSIONS];
  50. float accel[MOTION_DIMENSIONS];
  51. float maxVel = 1;
  52. float maxAccel = 1;
  53. int startMillis;
  54. // ROLLOVER WARNING: It appears that the Arduino counter rolls over
  55. // every 49 days, or 9 hours, depending on where you look. Rollover
  56. // is not handled right now, so at 9 hours or 49 days, don't know
  57. // which, of run time, the platform will do something weird. This is
  58. // on the list of things to fix.
  59. // http://www.faludi.com/2007/12/18/arduino-millis-rollover-handling/
  60.  
  61.  
  62. /* ********  M A I N   A R D U I N O   F U N C T I O N S  ******** */
  63.  
  64. /**
  65.  * Run when Arduino starts up or new serial connection is established.
  66.  */
  67. void setup(){
  68.   setupSPI();
  69.   setupSerial();
  70.   setupSmoothing();
  71. }
  72.  
  73. /**
  74.  * Runs continuously.
  75.  */
  76. void loop(){
  77.   readSerial();
  78.   interpolatePosition(millis());
  79. }
  80.  
  81.  
  82.  
  83.  
  84. /* ********  S E R I A L  ******** */
  85.  
  86. /**
  87.  * Setup the serial communication with the host computer
  88.  */
  89. void setupSerial(){
  90.   // Zero out the buffer
  91.   for( int i = 0; i <= SERIAL_BUFFER_SIZE; i++ ){
  92.     serialBuffer[i] = 0;
  93.   }
  94.   Serial.begin(BAUD);
  95.   sendInstructions();
  96. }
  97.  
  98.  
  99.  
  100. /**
  101.  * Send instructions via serial to the user.
  102.  */
  103. void sendInstructions(){
  104.   Serial.println("Commands:");
  105.   Serial.println(" r ..   Set roll from -1..+1");
  106.   Serial.println(" p ..   Set pitch from -1..+1");
  107.   Serial.println(" s v .. Set max velocity for moving the platform.");
  108.   Serial.println("        The units refer to the -1..+1 range of motion.");
  109. }
  110.  
  111.  
  112.  
  113. /**
  114.  * Reads the serial port for incoming commands from the user.
  115.  * When end of line is reached, the command is passed to the
  116.  * function processIncomingCommand(..).
  117.  */
  118. void readSerial(){
  119.  
  120.   if (Serial.available() > 0) {
  121.     int inByte = Serial.read();
  122. //    if( inByte >= 0 ) Serial.write(inByte);
  123.  
  124.     // No data
  125.     if( inByte < 0 ){
  126.       Serial.println("NO DATA EVEN THOUGH Serial.available() SAID YES");
  127.       delay(5);
  128.     }
  129.  
  130.     // End of line
  131.     // We're not always getting end of line characters, so optionally
  132.     // support ending a line with a semicolon. Yuck.
  133.     else if( (inByte == 10) || (inByte == 13) || (inByte == 59) ){ // ; = 59
  134.       Serial.println("<<< END OF LINE >>>");
  135.       serialBuffer[ serialBufferPosition ] = 0;
  136.       processIncomingCommand(serialBuffer);
  137.       serialBufferPosition = 0;
  138.     }
  139.    
  140.     // Request for instructions
  141.     else if( (inByte == 63) ){ // ? = 63
  142.       sendInstructions();
  143.     }
  144.  
  145.     // Save next input byte in the line
  146.     else {
  147.       serialBuffer[ serialBufferPosition ] = (char)inByte;
  148.       serialBufferPosition = (serialBufferPosition+1) % SERIAL_BUFFER_SIZE;
  149.     }
  150.   } // end if: serial available
  151.  
  152.  
  153. //Serial.print("SERIAL BUFFER POS: " ); Serial.println(serialBufferPosition);
  154. }
  155.  
  156.  
  157. /* ********  C O M M A N D S  ******** */
  158.  
  159.  
  160. /**
  161.  * Process an incoming command from the user.
  162.  * Commands are line-oriented, intended to be entered,
  163.  * at least during development, from a serial console.
  164.  */
  165. void processIncomingCommand( char* line ){
  166.   switch(line[0]){
  167.   case 'R':
  168.   case 'r':
  169.     handleCommand_R(line);
  170.     break; // Roll
  171.   case 'P':
  172.   case 'p':
  173.     handleCommand_P(line);
  174.     break; // Pitch
  175.   case 'S':
  176.   case 's':
  177.     handleCommand_S(line);
  178.     break; // Settings
  179.   default:
  180.     // Blink
  181.     setDAC(ROLL,+1);
  182.     delay(100);
  183.     setDAC(ROLL,0);
  184.     delay(100);
  185.     setDAC(ROLL,+1);
  186.     delay(100);
  187.     setDAC(ROLL,0);
  188.     delay(100);
  189.     setDAC(ROLL,+1);
  190.     delay(100);
  191.     setDAC(ROLL,0);
  192.     delay(100);
  193.     break;
  194.   }
  195. }
  196.  
  197. /**
  198.  * Set Roll with an ascii string, a floating point value -1..+1
  199.  */
  200. void handleCommand_R( char* line ){
  201.   if( line[0] == NULL ) return;   // Make sure it's not a null string
  202.  
  203.   char * tok = strtok(line, " "); // Should be 'R'
  204.   tok = strtok(NULL," ");         // Call again to skip over the first token (R)
  205.  
  206.   while (tok != NULL) {
  207.     float f = atof(tok);
  208.     setRoll(f);
  209.  
  210.     tok = strtok(NULL," ");
  211.   }
  212. }
  213.  
  214. /**
  215.  * Set Pitch with an ascii string, a floating point value -1..+1
  216.  */
  217. void handleCommand_P( char* line ){
  218.   if( line[0] == NULL ) return;   // Make sure it's not a null string
  219.  
  220.   char * tok = strtok(line, " "); // Should be 'P'
  221.   tok = strtok(NULL," ");         // Call again to skip over the first token (P)
  222.   while (tok != NULL) {
  223.     float f = atof(tok);
  224.     setPitch(f);
  225.  
  226.     tok = strtok(NULL," ");
  227.   }
  228. }
  229.  
  230.  
  231. /**
  232.  * Change settings.
  233.  */
  234. void handleCommand_S( char* line ){
  235.   if( line[0] == NULL ) return;   // Make sure it's not a null string
  236.  
  237.   char * tok = strtok(line, " "); // Tokenize the line (should return S)
  238.  
  239.   tok = strtok(NULL," ");         // What setting to change?
  240.   switch( tok[0] ){
  241.   case 'V':
  242.   case 'v':
  243.     tok = strtok(NULL," ");
  244.     float f = atof(tok);
  245.     maxVel = f;
  246.     Serial.print("Max velocity: ");
  247.     Serial.println(maxVel);
  248.     break;
  249.   } // end switch: setting
  250.  
  251. } // end command S
  252.  
  253.  
  254.  
  255. /* ********  P L A T F O R M   P O S I T I O N  ******** */
  256.  
  257. void setupSmoothing(){
  258.   startMillis = millis();
  259.   for( int i = 0; i < MOTION_DIMENSIONS; i++ ){
  260.     curr[i]  = 0;
  261.     goal[i]  = 0;
  262.     start[i] = 0;
  263.     vel[i]   = 0;
  264.     accel[i] = 0;
  265.   }
  266. }
  267.  
  268.  
  269. /**
  270.  * Sets the new requested roll value.
  271.  * Platform will begin to move toward that value.
  272.  */
  273. void setRoll( float minusOneToPosOne ){
  274. //  Serial.print("Setting roll to ");Serial.println(minusOneToPosOne);
  275.   setDimension(ROLL, minusOneToPosOne);
  276. }
  277.  
  278. /**
  279.  * Sets the new requested roll value.
  280.  * Platform will begin to move toward that value.
  281.  */
  282. void setPitch( float minusOneToPosOne ){
  283.   setDimension(PITCH, minusOneToPosOne);
  284. }
  285.  
  286. /**
  287.  * Consolidating function for setRoll and setPitch.
  288.  */
  289. void setDimension( unsigned int dim, float minusOneToPosOne ){
  290.   startMillis = millis();
  291.   minusOneToPosOne = min(1, max(-1, minusOneToPosOne)); // Cap it
  292.   start[dim] = curr[dim];
  293.   goal[dim] = minusOneToPosOne;
  294.   updateVelocity();
  295. /*    Serial.print("\nRoll: "); Serial.println( minusOneToPosOne );
  296.     Serial.print("\nStart: "); Serial.println( start[dim] );
  297.     Serial.print("\nCurr: "); Serial.println( curr[dim] );
  298.     Serial.print("\nGoal: "); Serial.println( goal[dim] );
  299.     Serial.print("\nVelocity: "); Serial.print( vel[0] ); Serial.print(", "); Serial.println( vel[1] );
  300. */    
  301. }
  302.  
  303.  
  304. /**
  305.  * After roll and pitch are set, the velocities
  306.  * need to be updated, so that the board moves
  307.  * in the new direction.
  308.  */
  309. void updateVelocity(){
  310.   for( int i = 0; i < MOTION_DIMENSIONS; i++ ){
  311.     vel[i] = goal[i] - start[i];
  312.   }
  313.  
  314.   normalizeVector(vel, MOTION_DIMENSIONS);
  315.   scaleVector(vel, MOTION_DIMENSIONS, maxVel);
  316. }
  317.  
  318.  
  319. /**
  320.  * Takes a vector and normalizes to unit vector (length one).
  321.  */
  322. void normalizeVector( float* vec, unsigned int length ){
  323.   float accum = 0;
  324.   for( int i = 0; i < length; i++ ){
  325.     accum += vec[i]*vec[i];
  326.   }
  327.   accum = sqrt(accum);
  328.   for( int i = 0; i < length; i++ ){
  329.     vec[i] = vec[i] / accum;
  330.   }
  331. }
  332.  
  333. void scaleVector( float* vec, unsigned int length, float scalar ){
  334.   for( int i = 0; i < length; i++ ){
  335.     vec[i] *= scalar;
  336.   }
  337. }
  338.  
  339.  
  340.  
  341. void interpolatePosition(int atMillis){
  342.   // If we're fed a timestamp that is too early, just give starting point
  343.   if( atMillis < startMillis ){
  344.     for(int i=0; i<MOTION_DIMENSIONS; i++){
  345.       curr[i] = start[i];
  346.     }
  347.   }
  348.   else {
  349.  
  350.     // Calculate based on timestamp and velocity
  351.     for(int i=0; i<MOTION_DIMENSIONS; i++){
  352.       curr[i] = start[i] + vel[i]*((atMillis-startMillis)*.001);
  353.     }
  354.  
  355.     for(int i=0; i<MOTION_DIMENSIONS; i++){
  356.       // If we're moving in the positive direction, and we've gone too far
  357.       if( start[i] <= goal[i] && goal[i] <= curr[i] ){
  358.         curr[i] = goal[i];
  359.       }
  360.       else
  361.  
  362.           // If we're moving in the negative direction, and we've gone too far
  363.         if( goal[i] <= start[i] && curr[i] < goal[i] ){
  364.           curr[i] = goal[i];
  365.         }
  366.     } // end for: each dimension
  367.  
  368.   }
  369.  
  370.   for(int i=0; i<MOTION_DIMENSIONS; i++){
  371.     setDAC(i, curr[i]);
  372. //      char c[50]; sprintf(c,"Curr: (%d, %d)", (int)(curr[0]*1000), (int)(curr[1]*1000)); Serial.println( c );
  373.   }
  374. }
  375.  
  376.  
  377.  
  378. /* ********  S P I   A N D   V O L T A G E  ******** */
  379.  
  380. // http://www.arduino.cc/en/Tutorial/SPIEEPROM
  381. // http://www.arduino.cc/en/Tutorial/SPIDigitalPot
  382.  
  383.  
  384. void setupSPI(){
  385.   resetDAC();
  386.   pinMode(SPI_DATA_OUT, OUTPUT);        // SPI Output
  387.   pinMode(SPI_DATA_IN, INPUT);          // SPI Input
  388.   pinMode(SPI_CLOCK,OUTPUT);            // SPI Clock
  389.   pinMode(SPI_SLAVE_SELECT_1,OUTPUT);   // SPI Slave deactivated
  390.   digitalWrite(SPI_SLAVE_SELECT_1,HIGH);
  391.  
  392.  
  393.   // SPCR = 01010000
  394.   //interrupt disabled,spi enabled,msb 1st,master,clk low when idle,
  395.   //sample on leading edge of clk,system clock/4 rate (fastest)
  396.   // SPCR
  397.   // | 7 | 6 | 5 | 4 | 3 | 2 | 1 | 0 |0000000000000000000
  398.   // | SPIE | SPE | DORD | MSTR | CPOL | CPHA | SPR1 | SPR0 |
  399.  
  400.   // SPIE - Enables the SPI interrupt when 1
  401.   // SPE - Enables the SPI when 1
  402.   // DORD - Sends data least Significant Bit First when 1, most Significant Bit first when 0
  403.   // MSTR - Sets the Arduino in master mode when 1, slave mode when 0
  404.   // CPOL - Sets the data clock to be idle when high if set to 1, idle when low if set to 0
  405.   // CPHA - Samples data on the falling edge of the data clock when 1, rising edge when 0'
  406.   // SPR1 and SPR0 - Sets the SPI speed, 00 is fastest (4MHz) 11 is slowest (250KHz)
  407.   SPCR = 0 |
  408.     (0<<7) |  // SPIE: disables the SPI interrupt
  409.   (1<<6) |  // SPE:  enables the SPI
  410.   (0<<5) |  // DORD: chooses transmission with the most significant bit going first
  411.   (1<<4) |  // MSTR: puts the Arduino in Master mode
  412.   (0<<3) |  // CPOL: sets the data clock idle when it is low
  413.   (0<<2) |  // CPHA: sets the SPI to sample data on the rising edge of the data clock
  414.   (0<<1) |  // SPR1: second and first bits set the speed of the
  415.   (0   );   // SPR0: SPI to system speed / 4 (the fastest)
  416.   //SPCR = (1<<SPE) | (1<<MSTR);
  417.   SPSR = B00000000;              // not double data rate
  418.  
  419.   byte clr=SPSR;
  420.   clr=SPDR;
  421.   delay(10);
  422.  
  423. } // end setupSPI
  424.  
  425. /**
  426.  * This resets the DAC hardware.
  427.  */
  428. void resetDAC(){
  429.   pinMode(DAC_CLEAR_PIN, OUTPUT);      
  430.   digitalWrite(DAC_CLEAR_PIN,LOW);
  431.   delay(5);
  432.   digitalWrite(DAC_CLEAR_PIN,HIGH);
  433. }
  434.  
  435. /**
  436.  * Sets the voltage output based on one of the known
  437.  * dimensions (roll and pitch at this point) and an
  438.  * extent of that dimension scaled from -1 to +1.
  439.  */
  440. void setDAC( unsigned int dimension, float minusOneToPositiveOne ){
  441.  
  442.   unsigned int SS_PIN = -1;
  443.   switch( dimension ){
  444.   case ROLL:
  445.     SS_PIN = ROLL_PIN;
  446.     break;
  447.   case PITCH:
  448.     SS_PIN = PITCH_PIN;
  449.     break;
  450.   default:
  451.     Serial.print("\nError in setDAC function: Unknown dimension ");
  452.     Serial.println(dimension);
  453.     return;
  454.   }
  455. //  Serial.print("Value: " ); Serial.println( minusOneToPositiveOne );
  456.  
  457.   if( minusOneToPositiveOne > 0 ){
  458.     // Positive values
  459.     float f = min(minusOneToPositiveOne,1);
  460.     int spiVal = max(0.0, min( 63.0, f*64));
  461.     digitalWrite(SS_PIN,LOW);
  462.     spi_transfer_16( spiVal << 8 );
  463.     digitalWrite(SS_PIN,HIGH);
  464. //if( dimension == 0 ){    Serial.print("Pos, Pin: " ); Serial.print(SS_PIN); Serial.print(", Value: "); Serial.println(spiVal << 8); }
  465.   }
  466.   else if( minusOneToPositiveOne < 0 ){
  467.     // Negative values
  468.     float f = -max(minusOneToPositiveOne,-1);
  469.     int spiVal = 127-max(0.0, min( 63.0, f*64));
  470.     digitalWrite(SS_PIN,LOW);
  471.     spi_transfer_16( spiVal << 8 );
  472.     digitalWrite(SS_PIN,HIGH);
  473. //if( dimension == 0 ){    Serial.print("Neg, Pin: " ); Serial.print(SS_PIN); Serial.print(", Value: "); Serial.println(spiVal << 8); }
  474.   }
  475.   else {
  476.     // Zero
  477.     digitalWrite(SS_PIN,LOW);
  478.     spi_transfer_16( 0 );
  479.     digitalWrite(SS_PIN,HIGH);
  480. //if( dimension == 0 ){    Serial.print("Zero, Pin: " ); Serial.print(SS_PIN); Serial.print(", Value: "); Serial.println(0); }
  481.   }
  482. }
  483.  
  484.  
  485.  
  486. void spi_transfer_16(volatile int data){
  487.   spi_transfer(data>>8);
  488.   spi_transfer(data);
  489. }
  490.  
  491. char spi_transfer(volatile char data)
  492. {
  493.   SPDR = data;                    // Start the transmission
  494.   while (!(SPSR & (1<<SPIF))) ;   // Wait for the end of the transmission
  495.   return SPDR;                    // return the received byte
  496. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!