SHARE
TWEET

Untitled

a guest Jan 16th, 2019 78 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include "PidServer.h"
  2.  
  3. /**
  4.  *  @brief This function handles incoming HID packets from MATLAB.
  5.  *
  6.  *  @description This method has two parts: in part 1, we will decode the incoming
  7.  *               packet, extract the setpoints and send those values to the
  8.  *               PID controller; in part 2, we will generate a response that will be
  9.  *               sent back to MATLAB through HID. This is useful to e.g. send sensor
  10.  *               data to MATLAB for plotting.
  11.  */
  12. void PidServer::event(float * packet){
  13.  
  14.   /*
  15.    * ======= PART 1: Decode setpoints and send commands to the PID controller ==
  16.    */
  17.  
  18.   bool skipLink = false; //!FIXME Do we need this? If not, let's get rid of it
  19.  
  20.   for (int i = 0; i < myPumberOfPidChannels; i++)
  21.     {
  22.       // extract the three setpoint values (one for each joint) from the packet buffer
  23.       float setpoint = packet[(i*3)+0];
  24.       float velocityTarget = 0; // this is currently unused
  25.       float forceTarget = 0;    // this is currently unused
  26.       //printf("\r\n %i : %f", i,setpoint);
  27.       // get current position from arm
  28.       float position = myPidObjects[i]->GetPIDPosition();
  29.  
  30.       // now let's initiate motion to the setpoints
  31.  
  32.       // !FIXME I am not sure what the next two instructions are for.
  33.       //        The if statement below always returns false and therefore we never
  34.       //        enter the clause. Is this code needed? If not, let's get rid of it.
  35.       float timeOfMotion = 0;
  36.       if(std::abs(velocityTarget)>0)
  37.     timeOfMotion=(std::abs(setpoint-position)/velocityTarget)*1000;// convert from Tics per second to miliseconds
  38.       // When polling for the current positon, we are passing down setpoints over and over. If the setpoint is already set we want to skip
  39.       // Bound function is checking the incoming value agains the previouslt set value
  40.       bool newUpdate = !myPidObjects[i]->bound(setpoint,
  41.                            myPidObjects[i]->state.interpolate.set,
  42.                            0.01,   // is the incoming setpoint plus 0.01 from the last setpoint
  43.                            0.01);// is the incoming setpoint minus 0.01 from the last setpoint
  44.       // If the incoming value is outside of the previous value, then we actually set the PID controller
  45.       if(newUpdate)
  46.     {
  47.       // disable interrupts first
  48.       __disable_irq();
  49.       myPidObjects[i]->SetPIDEnabled(true); // !FIXME Do we need to do this
  50.                                             //  every time?
  51.                                             // Can't we just leave it enabled?
  52.  
  53.       // go to setpoint in timeOfMotion ms, linear interpolation
  54.       myPidObjects[i]->SetPIDTimed(setpoint, timeOfMotion);
  55.       // re-enable interrupts
  56.     __enable_irq();
  57.  
  58.     }
  59.  
  60.       else // This is what happens if the setpoint is the same as the cutrrent setpoint
  61.        //        this is only called when the polling packet is just reading
  62.     {
  63.       //  printf("\r\nPacket write ignored, index %i to %f is already %f",i,setpoint,myPidObjects[i]->state.interpolate.set);
  64.       skipLink=true;
  65.     }
  66.       //  if(skipLink){
  67.       //    for (int i=0;i<15;i++){
  68.       //      printf("\r\nPacket write ignored, value %i to %f ",i,packet[i]);
  69.       //    }
  70.       //}
  71.     }
  72.  
  73.   /*
  74.    * ======= PART 2: Generate a response to be sent back to MATLAB =============
  75.    */
  76.  
  77.   // we will be using the same memory area in which the incoming packet was stored,
  78.   // however, a we need to perform a type cast first (for convenience).
  79.   uint8_t * buff = (uint8_t *) packet;
  80.  
  81.   // re-initialize the packet to all zeros
  82.   for (int i = 0; i < 60; i++)
  83.       buff[i] = 0;
  84.  
  85.   /**
  86.    * The following loop reads sensor data (encoders ticks, joint velocities and
  87.    * force readings) and writes it in the response packet.
  88.    */
  89.   for(int i = 0; i < myPumberOfPidChannels; i++)
  90.     {
  91.       packet[(i*3)+0] = 0;
  92.       packet[(i*3)+1] = 0;
  93.       packet[(i*3)+2] = 0;
  94.     }
  95. }
RAW Paste Data
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!
 
Top