void ee_writeStr(char *s, int n, int addr)
{
  while(lockset(eei2cLock));
  ee_putStr(s, n, addr);
  lockclr(eei2cLock);
  return;
}
void init_MMA7660FC(void)
{
  int x, y, z;
  unsigned char val = 0;
  bt_accelInitFlag = 1;
  if(!eei2cLockFlag)
  {
    leds(0b111111);
    eei2cLock = locknew();
    lockclr(eei2cLock);
    eei2cLockFlag = 1;
  }
  if(!st_eeInitFlag) ee_init();
  while(lockset(eei2cLock));  
  i2c_out(st_eeprom, MMA7660_I2C, 
          MODE, 1, &val, 1);
  i2c_out(st_eeprom, MMA7660_I2C, 
          INTSU, 1, &val, 1);
  i2c_out(st_eeprom, MMA7660_I2C, 
          SR, 1, &val, 1);
  val = 0xC1;
  i2c_out(st_eeprom, MMA7660_I2C, 
          MODE, 1, &val, 1);
  i2c_stop(st_eeprom);        
  lockclr(eei2cLock);
  accels(&x, &y, &z);        
}
short ee_readShort(int addr)
{
  while(lockset(eei2cLock));
  int value;
  i2c_in(st_eeprom, 0x50, addr, 2, (unsigned char*) &value, 2);
  lockclr(eei2cLock);
  return value;
}
Esempio n. 4
0
uint8_t I2CBusy(uint8_t address)
{
    uint8_t result;
    
    lockset(lock);
    result = i2c_busy(i2c_bus, address);
    lockclr(lock);
    return result;
}
Esempio n. 5
0
void SendMessage(char* buffer)
{
    PEND_ON_BUFFER(outgoing_msg);
    lockset(lock);
    memcpy((char *) outgoing_msg.data, buffer, strlen(buffer));
    outgoing_msg.data[strlen(buffer)] = 0; // Null terminate the data
    outgoing_msg.is_full = 1;
    lockclr(lock);
}
Esempio n. 6
0
/* Sets the ramp rate.
 */
void Servo::setRamp(int16_t stepSize)
{
  // Set the lock
  while(lockset(Servo::_lockID));

  Servo::_slots[_idx]._stepSize = stepSize;

  // Clear the lock before return.
  lockclr(Servo::_lockID);
}
Esempio n. 7
0
void Servo::writeMicroseconds(int16_t value)
{
  // Set the lock
  while(lockset(Servo::_lockID));

  Servo::_slots[_idx]._currentPulse = value;

  // Clear the lock before return.
  lockclr(Servo::_lockID);
}
Esempio n. 8
0
static void servo(void *par)                         // servo process in other cog
{
  int dtpw = (CLKFREQ/1000000)*2500;
  int pw = CNT;

  int dtTw = (CLKFREQ/1000000)*20000;
  int Tw = pw;
  Tw += dtTw;

  int s = sizeof(p)/sizeof(int);                     // Get size of servo array
  int i;                                             // Local index variable
  while(1)                                           // servo control loop
  {
    while(lockset(lockID));                          // Set the lock 
    for(i = 0; i < s; i++)                           // Go through all possible servos
    {
      if(t[i] == 0)                                  // Detach servo? 
      {
        input(p[i]);                                 // Set I/O pin to input
        p[i] = -1;                                   // Remove from list
        t[i] = -1;
        tp[i] = -1;
        r[i] = 2000;
      }
      if(p[i] != -1)                                 // If servo entry in pin array
      {
        int tPulse =  t[i];                          // Copy requested position to var
        int diff = tPulse - tp[i];                   // Check size of change
        int d = abs(diff);                           // Take absolute value
        if(r[i] < d)                                 // If change larger than ramp step
        {
          int step = r[i];                           // Copy step entry to variable
          if(diff < 0)                               // If negative
          {
            step = -step;                            // Make step negative
          }
          tPulse = tp[i] + step;                     // Increment pulse by step 
        }
        pulse_outCtr(p[i], tPulse);                  // Send pulse to servo
        tp[i] = tPulse;                              // Remember pulse for next time
      }
      if(i%2)
      {
         while((CNT - pw) <= dtpw);                  // Wait for servo pulse window to close
         pw += dtpw;
      }
    }
    lockclr(lockID);                                 // Clear the lock
    while((CNT - pw) <= dtpw);                       // Wait for 20 ms since first pulse
    pw += dtpw;
  }
}
int accel(int axis)
{
  if(!st_eeInitFlag) ee_init();
  unsigned char val = 0;
  while(lockset(eei2cLock));
  i2c_in (st_eeprom, MMA7660_I2C, 
          axis, 1, &val, 1);
  i2c_stop(st_eeprom);        
  lockclr(eei2cLock);
  int g100 = raw2g100(val);
  if(axis == AY) g100 = -g100;
  return g100;
}
Esempio n. 10
0
/* Returns true if the pin has a value.
 */
bool Servo::attached()
{
  bool retVal;

  // Set the lock
  while(lockset(Servo::_lockID));

  // capture the current pulse value.
  retVal = Servo::_slots[_idx]._pin != -1 ? true : false;

  lockclr(Servo::_lockID);

  return retVal;
}
Esempio n. 11
0
/* Returns the current pulse value in microseconds, note that if ramping
 * is in use this value might not be the value being sent to the servo.
 */
int16_t Servo::readMicroseconds()
{
  int retVal = -1;

  // Set the lock
  while(lockset(Servo::_lockID));

  // capture the current pulse value.
  retVal = Servo::_slots[_idx]._currentPulse;

  lockclr(Servo::_lockID);

  return retVal;
}
Esempio n. 12
0
uint8_t AcquireMessage(char *buffer)
{
    if (incoming_msg.is_full)
    {
        lockset(lock);
        memcpy(buffer, (char *) incoming_msg.data, incoming_msg.count);
        //incoming_msg.data[incoming_msg.count + 1] = 0;
        incoming_msg.is_full = 0;
        lockclr(lock);
        return 1;
    }
    
    return 0;
}
Esempio n. 13
0
/* servo tending cog method.
 */
void Servo::servoLoop(void *par)
{
  // Servo control loop runs until stopped.
  while(true)                                 
  {
    // For each servo.
    for(uint8_t i = 0; i < MAX_SERVOS; i++)
    {
      // Set the lock
      while(lockset(Servo::_lockID));

      // If this servo slot is bound to a pin.
      if (Servo::_slots[i]._pin != -1)
      {

        // Copy requested position to var
        int16_t tPulse = Servo::_slots[i]._currentPulse;

        // Compute the required size of change.
        int16_t diff = tPulse - Servo::_slots[i]._previousPulse;

        // If change larger than ramp step
        if (Servo::_slots[i]._stepSize < abs(diff))                          
        {
          int16_t step = Servo::_slots[i]._stepSize;
          // If the difference is negative, then make the step negative.
          if (diff < 0)
          {
            step = -step;
          }

          // Increment pulse by step to get closer to target.
          tPulse = Servo::_slots[i]._previousPulse + step;              
        }

        // Send pulse to servo and remember pulse for next time
        pinMode(Servo::_slots[i]._pin, OUTPUT);
        pulseOut(Servo::_slots[i]._pin, tPulse);
        Servo::_slots[i]._previousPulse = tPulse;
      }

      // Clear the lock.
      lockclr(Servo::_lockID);
    }

    // Sleep until next pulse required.
    delay(20);
  }
}
Esempio n. 14
0
int32_t test29::Str(int32_t Stringptr)
{
  while (lockset(Strlock)) {
    Yield__();
  }
  {
    int32_t _idx__0000;
    int32_t _limit__0001 = strlen((char *) Stringptr);
    for(_idx__0000 = 1; _idx__0000 <= _limit__0001; (_idx__0000 = (_idx__0000 + 1))) {
      Tx(((uint8_t *)(Stringptr++))[0]);
    }
  }
  lockclr(Strlock);
  return 0;
}
Esempio n. 15
0
int servo_setramp(int pin, int stepSize)             // Set ramp step for a servo
{
  int s = sizeof(p)/sizeof(int);                     // Get array size
  int i;                                             // Local index variable
  while(lockset(lockID));                            // Set lock
  for(i = 0; i < s; i++)                             // Find index for servo pin
  {
    if(p[i] == pin)                                  // Found pin in array?
    {
      r[i] = stepSize;                               // Set ramp step
      lockclr(lockID);                               // Clear lock
      return pin;                                    // Return success
    }
  }
  lockclr(lockID);                                   // Clear lock
  return -4;                                         // Return -1, pin not found
}
int32_t screen_update(void)
{
  while(lockset(screenLock));
  int32_t _local__0014[2] = {0, 0};
  // Writes the screen buffer to the memory of the display
  // low col = 0
  screen_ssd1306_Command(SSD1306_SETLOWCOLUMN);
  // hi col = 0
  screen_ssd1306_Command(SSD1306_SETHIGHCOLUMN);
  // line #0
  screen_ssd1306_Command(SSD1306_SETSTARTLINE);
  screen_HIGH(self->DC);
  screen_WRITEBUFF(self->DATA, self->CLK, self->CS, (int32_t)(&_local__0014[1]), (int32_t)(&self->buffer[0]));
  //screen_WRITEBUFF(self->DATA, self->CLK, self->CS, 8192, (int32_t)(&self->buffer[0]));
  screen_LOW(self->DC);
  lockclr(screenLock);
  return 0;
}
Esempio n. 17
0
uint16_t I2C_ReadADC(uint8_t write_addr, uint8_t read_addr, uint8_t channel)
{
    uint8_t a2d_val_high;
    uint8_t a2d_val_low;
    
    int ack;
    
    lockset(lock);
    ack = i2c_startpoll(&i2c_bus, write_addr);    
    ack = i2c_writeByte(&i2c_bus, ADC_CHANNEL[channel]);
    ack = i2c_startpoll(&i2c_bus, read_addr);    
    a2d_val_high = i2c_readByte(&i2c_bus, 0);
    a2d_val_low = i2c_readByte(&i2c_bus, 1);
    i2c_stop(&i2c_bus);
    lockclr(lock);
        
    return (((uint16_t) a2d_val_high) << 8) | (uint16_t) a2d_val_low;
}
Esempio n. 18
0
void I2C_ByteWrite(I2C_ADDR_t slaveAddress, uint8_t reg, uint8_t data)
{
    uint8_t num_bytes;
    uint8_t size = 0;
    
    if (reg > 0)
    {
        size = sizeof(reg);
    }

    lockset(lock);
    if (I2C_Ready(slaveAddress))
    {
        num_bytes = i2c_out(i2c_bus, slaveAddress, reg, size, &data, sizeof(data));
    }    
    lockclr(lock);
    print("\t\tWrote - addr %02x, reg %02x, data %02x, total %d\n", slaveAddress, reg, data, num_bytes);
}
Esempio n. 19
0
int servo_set(int pin, int time)               // Set pulse width to servo on pin 
{
  if(servoCog == 0)                           // If cog not started
  {
    int result = servo_start();                // Start the cog
    if(result == 0) return 0;                 // No cogs open
    if(result == -1) return -1;               // No locks open
  }
  int s = sizeof(p)/sizeof(int);              // Array size to s
  int i;                                      // Index variable
  while(lockset(lockID));                     // Set the lock
  for(i = 0; i < s; i++)                      // Check if existing servo
  {
    if(p[i] == pin)                           // Yes?
    {
      t[i] = time;                            // Set pulse duration
      lockclr(lockID);                        // Clear lock
      return 1;                               // Return success 
    }
  }
  if(i == s)                                  // Not existing servo?
  {
    for(i= 0; i < s; i++)                     // Look for empty slot
    {
      if(p[i]==-1)                            // Found one?
      {
        break;                                // Exit for loop, keep index
      }
    }
    if(i <= s)                                // Found empty slot?
    {
      p[i] = pin;                             // Set up pin and pulse durations
      t[i] = time;
      tp[i] = time;
      lockclr(lockID);                        // Clear the lock 
      return 1;                               // Return success 
    }
    else                                      // Servo not found, no empty slots?
    {
      lockclr(lockID);                        // Clear lock
      return 0;                               // Return, set not completed
    }
  }
}
Esempio n. 20
0
void I2C_ByteRead(I2C_ADDR_t slaveAddress, uint8_t reg, uint8_t* data)
{
    uint8_t num_bytes;
    uint8_t size = 0;
    
    if (reg > 0)
    {
        size = sizeof(reg);
    }
    
    lockset(lock);
    while (i2c_busy(i2c_bus, slaveAddress))
    {
        ;
    }    
    num_bytes = i2c_in(i2c_bus, slaveAddress, reg, size, data, 1);
    lockclr(lock);
    print("\t\tRead addr %02x, reg %02x, data %02x, total %d\n", slaveAddress, reg, *data, num_bytes);
}
Esempio n. 21
0
void servo(void *par)                         // Servo process in other cog
{
  int s = sizeof(p)/sizeof(int);              // Get size of servo array
  int i;                                      // Local index variable
  mark();                                     // Mark the current time
  while(1)                                    // Servo control loop
  {
    while(lockset(lockID));                   // Set the lock 
    for(i = 0; i < s; i++)                    // Go through all possible servos
    {
      if(t[i] == 0)                           // Detach servo? 
      {
        input(p[i]);                          // Set I/O pin to input
        p[i] = -1;                            // Remove from list
        t[i] = -1;
      }
      if(p[i] != -1)                          // If servo entry in pin array
      {
        int tPulse =  t[i];                   // Copy requested position to var
        int diff = tPulse - tp[i];            // Check size of change
        int d = abs(diff);                    // Take absolute value
        if(r[i] < d)                          // If change larger than ramp step
        {
          int step = r[i];                    // Copy step entry to variable
          if(diff < 0)                        // If negative
          {
            step = -step;                     // Make step negative
          }
          tPulse = tp[i] + step;              // Increment pulse by step 
        }
        pulse_out(p[i], tPulse);               // Send pulse to servo
        tp[i] = tPulse;                       // Remember pulse for next time
      }
    }
    lockclr(lockID);                          // Clear the lock
    wait(20000);
  }
}
Esempio n. 22
0
/* Constructor which creates one lock on first construction.
 * This is not completely thread safe, but these are created
 * during program setup before backgroup processing starts.
 */
Servo::Servo()
{
  // Now we're thread safe, so set the lock.
  while(lockset(Servo::_lockID));

  // if we still have free slots.
  if( Servo::_servoCount < MAX_SERVOS)
  {
    // Grab a slot in the properties array.
    _idx = Servo::_servoCount;

    // Increment the servo count.
    Servo::_servoCount++;
  }
  else
  {
    // too many servos 
    _idx = INVALID_SERVO;
  }

  // Clear lock
  lockclr(Servo::_lockID);
}
Esempio n. 23
0
/* Binds a pin to this servo object and sets the limits.
 */
uint8_t Servo::attach(int16_t pin, int16_t min, int16_t max)
{
  // capture the config for future write methods.
  _min  = (MIN_PULSE_WIDTH - min)/4; //resolution of min/max is 4 uS
  _max  = (MAX_PULSE_WIDTH - max)/4; 

  // Set lock before touching the shared properties array
  while(lockset(Servo::_lockID));

  // If cog not started, start it.
  if (Servo::_servoCog == 0)
  {
    Servo::_servoCog = cogstart(&servoLoop, NULL, stack, sizeof(stack)) + 1;
  }

  Servo::_slots[_idx]._pin = pin;
  pinMode(Servo::_slots[_idx]._pin, OUTPUT);

  // Increment when a servo is attached.
  Servo::_attachedCount++;

  // Clear lock
  lockclr(Servo::_lockID);
}
Esempio n. 24
0
/* Releases a pin from this servo object and resets the slot state.
 */
void Servo::detach()
{
  // Set lock.
  while(lockset(Servo::_lockID));

  Servo::_slots[_idx]._pin = -1;
  Servo::_slots[_idx]._currentPulse = DEFAULT_PULSE_WIDTH;
  Servo::_slots[_idx]._previousPulse = DEFAULT_PULSE_WIDTH;
  Servo::_slots[_idx]._stepSize = 2000;

  // Decrement the attached servo count.
  Servo::_attachedCount--;

  // if no more attached servos and the cog is running,
  // then stop the cog and record that it's stopped.
  if (Servo::_attachedCount == 0 && Servo::_servoCog != 0)
  {
    cogstop(Servo::_servoCog-1);
    Servo::_servoCog = 0;
  }

  // Clear lock
  lockclr(Servo::_lockID);
}
Esempio n. 25
0
void AsiMS2000::selectCommand(int commandNum)
{
  switch(commandNum)
  {
      case 0:
          accel();
          break;
      case 1:
          aalign();
          break;
      case 2:
          afcont();
          break;
      case 3:
          aflim();
          break;
      case 4:
          afocus();
          break;
      case 5:
          afset();
          break;
      case 6:
          afmove();
          break;
      case 7:
          ahome();
          break;
      case 8:
          aij();
          break;
      case 9:
          array();
          break;
      case 10:
          azero();
          break;
      case 11:
          backlash();
          break;
      case 12:
          bcustom();
          break;
      case 13:
          benable();
          break;
      case 14:
          build();
          break;
      case 15:
          cdate();
          break;
      case 16:
          cnts();
          break;
      case 17:
          customa();
          break;
      case 18:
          customb();
          break;
      case 19:
          dack();
          break;
      case 20:
          dump();
          break;
      case 21:
          ensync();
          break;
      case 22:
          epolarity();
          break;
      case 23:
          error();
          break;
      case 24:
          halt();
          break;
      case 25:
          here();
          break;
      case 26:
          home();
          break;
      case 27:
          info();
          break;
      case 28:
          joystick();
          break;
      case 29:
          jsspd();
          break;
      case 30:
          kadc();
          break;
      case 31:
          kd();
          break;
      case 32:
          ki();
          break;
      case 33:
          kp();
          break;
      case 34:
          lcd();
          break;
      case 35:
          led();
          break;
      case 36:
          lladdr();
          break;
      case 37:
          load();
          break;
      case 38:
          lock();
          break;
      case 39:
          lockrg();
          break;
      case 40:
          lockset();
          break;
      case 41:
          maintain();
          break;
      case 42:
          motctrl();
          break;
      case 43:
          move();
          break;
      case 44:
          movrel();
          break;
      case 45:
          pcros();
          break;
      case 46:
          pedal();
          break;
      case 47:
          rbmode();
          break;
      case 48:
          rdadc();
          break;
      case 49:
          rdsbyte();
          break;
      case 50:
          rdstat();
          break;
      case 51:
          relock();
          break;
      case 52:
          reset();
          break;
      case 53:
          rt();
          break;
      case 54:
          runaway();
          break;
      case 55:
          saveset();
          break;
      case 56:
          savepos();
          break;
      case 57:
          scan();
          break;
      case 58:
          scanr();
          break;
      case 59:
          scanv();
          break;
      case 60:
          secure();
          break;
      case 61:
          sethome();
          break;
      case 62:
          setlow();
          break;
      case 63:
          setup();
          break;
      case 64:
          si();
          break;
      case 65:
          speed();
          break;
      case 66:
          spin();
          break;
      case 67:
          status();
          break;
      case 68:
          stopbits();
          break;
      case 69:
          ttl();
          break;
      case 70:
          um();
          break;
      case 71:
          units();
          break;
      case 72:
          unlock();
          break;
      case 73:
          vb();
          break;
      case 74:
          vector();
          break;
      case 75:
          version();
          break;
      case 76:
          wait();
          break;
      case 77:
          where();
          break;
      case 78:
          who();
          break;
      case 79:
          wrdac();
          break;
      case 80:
          zero();
          break;
      case 81:
          z2b();
          break;
      case 82:
          zs();
          break;
      case 83:
          overshoot();
          break;
  }
}
Esempio n. 26
0
static void HandleSerial(void *par)
{
    int count;
    char next;
    
    while (1)
    {
        // Check for incoming message, read and store it, signal the message is ready
        if (!incoming_msg.is_full)
        { 
            if (fdserial_rxReady(msg_serial) != 0)
            {
                count = 0;
                
                while (count < sizeof(incoming_msg.data)) 
                {
                    next = readChar(msg_serial);
                    lockset(lock);
                    incoming_msg.data[count] = next;
#ifdef ECHO_SERIAL_MSG // Use this only for debugging problems with message handling
                    echo_msg.data[count] = next;
#endif                        
                    lockclr(lock);
                    
                    if (incoming_msg.data[count] == '\r' || incoming_msg.data[count] == '\n')
                    {
                        lockset(lock);
                        incoming_msg.count = count;
                        //incoming_msg[count + 1] = '\0';
                        incoming_msg.is_full = 1;
#ifdef ECHO_SERIAL_MSG // Use this only for debugging problems with message handling
                        echo_msg.count = count;
                        //echo_msg[count] = '\0';
                        echo_msg.is_full = 1;
#endif                        
                        lockclr(lock);
                        break;
                    }    
                    count++;
                }
            }
        }
        else
        {
            // This means we aren't processing incomming messages fast enough
            // Could implment an overflow counter that could be reported back as status
        }
        
#ifdef ECHO_SERIAL_MSG // Use this only for debugging problems with message handling
        lockset(lock);
        if (echo_msg.is_full)
        {
            dprint(msg_serial, "E:%s", echo_msg.data);
            echo_msg.is_full = 0;
        }            
        lockclr(lock);
#endif
        lockset(lock);
        // Check for outgoing message, send, and clear flag
        if (outgoing_msg.is_full)
        {
            dprint(msg_serial, "%s", outgoing_msg.data);
            outgoing_msg.is_full = 0;
        }
        lockclr(lock);
    }        
}