Mindboards

Code: Select all

// program Tetrix_Motor_Driver ver. 0.17


#define printf1( _x, _y, _format1, _value1) { \
  string sval1 = FormatNum(_format1, _value1); \
  TextOut(_x, _y, sval1); \
}

int LCDline[]={56,48,40,32,24,16,8,0};

#define HTNofMots  2         // number of motors = 2
#define idle  0
#define busy  1
#define brake 0
#define coast -128

#define TxMuxPort S1



char HTMotState[HTNofMots];    // motor status: idle, busy
char HTMotSetSpeed[HTNofMots]; // 0, -128, 1...100, -1...-100
char HTMotCmdSucc[HTNofMots];  // 0=false, 1=true
long HTMotEnc[HTNofMots];      // current motor encoder values


safecall void GetTXMotorCounter(byte NXTport, byte TXmotor)
{
  byte devAddr;
  byte sendMsg[];
  byte replyMsg[];
  const byte MsgLen = 4;
  byte encStartReg;

  devAddr = (TXmotor+2)&14;
  if (TXmotor==0) encStartReg = 0x4C;
  else
  if (TXmotor==1) encStartReg = 0x50;

  ArrayBuild(sendMsg, devAddr, encStartReg);
  // ArrayBuild(sendMsg, DGPS_I2C_ADDR, START_OF_ENCODER_REGISTER);   // Only -one- register, the first one, is requested

  while ((I2CCheckStatus(NXTport) == STAT_COMM_PENDING) && (I2CCheckStatus(NXTport) != NO_ERR)) Wait(1);

  // if(!I2CBytes(NXTport, sendMsg, msgLen, replyMsg)) return ;

  if(I2CBytes(NXTport, sendMsg, MsgLen, replyMsg)) {
     HTMotEnc[TXmotor] = replyMsg[3] + (replyMsg[2] << 8) + (replyMsg[1] << 16) + (replyMsg[0] << 24);
  }
}



inline void TXMotorOn(byte NXTport, byte TXmotor, char percentage)
{
  byte retLen = 0;
  char modeMsg[];
  char powerMsg[];
  byte devAddr, modeReg, powerReg;
  char IOresult;
  

  // addresses and registers
  devAddr = (TXmotor+2)&14;
  modeReg = 0x44 + (TXmotor % 2)*3;
  powerReg= 0x45 + (TXmotor % 2);

  ArrayBuild(modeMsg, devAddr, modeReg, 0);    // 0= PWM percentage mode
  ArrayBuild(powerMsg, devAddr, powerReg, percentage);
  
  // Send the first message as soon as the bus is ready

  while ((I2CCheckStatus(NXTport) == STAT_COMM_PENDING) && (I2CCheckStatus(NXTport) != NO_ERR)) Wait(10);
  NEW:
    IOresult=I2CWrite(NXTport, retLen, modeMsg);
    if (IOresult != NO_ERR) {
      Wait(5); goto NEW;
    }

  
  Wait(10);

  // Send the second message when the first one is done

  while ((I2CCheckStatus(NXTport) == STAT_COMM_PENDING) && (I2CCheckStatus(NXTport) != NO_ERR)) Wait(10);
  IOresult=I2CWrite(NXTport, retLen, powerMsg);

  if (IOresult != NO_ERR)   {  return;  }

  Wait(10);

}


inline void ResetTXMotorCounter(byte NXTport, byte TXmotor)
{
  byte retLen = 0;
  char modeMsg[];
  byte devAddr, modeReg;
  char IOresult;

  // addresses and registers
  devAddr = (TXmotor+2)&14;
  modeReg = 0x44 + (TXmotor % 2)*3;

  ArrayBuild(modeMsg, devAddr, modeReg, 3);      // 3 = reset enc value

   // Send the reset message as soon as the bus is ready

  while ((I2CCheckStatus(NXTport) == STAT_COMM_PENDING) && (I2CCheckStatus(NXTport) != NO_ERR)) Wait(1);
  IOresult=I2CWrite(NXTport, retLen, modeMsg);
  if (IOresult != NO_ERR)   { return;  };
  
  Wait(5);

}


task RefreshMotorEncValues(){
  while(true) {
     for (int i=0; i<HTNofMots; i++) {
       GetTXMotorCounter(TxMuxPort, i);
       Wait(20);
     }
  }
}


task DisplayValues() {
  long lbuf;
  while (true){
  
    lbuf=HTMotEnc[0]; printf1(0,LCDline[0],"Enc 0 =%5d", lbuf);
    lbuf=HTMotEnc[1]; printf1(0,LCDline[1],"Enc 1 =%5d", lbuf);
    Wait(20);
  }

}


task main(){

  SetSensorType(TxMuxPort, SENSOR_TYPE_LOWSPEED);
  Wait(20);
  ResetSensor(TxMuxPort);
  Wait(50);
  
  start DisplayValues;

  while (true) {                // test loop
    PlaySound (SOUND_DOUBLE_BEEP);

    stop RefreshMotorEncValues; Wait(20);
    TXMotorOn(TxMuxPort, 0, 50);
    TXMotorOn(TxMuxPort, 1, 20);
    start RefreshMotorEncValues;

    Wait(2000);

    stop RefreshMotorEncValues; Wait(50);
    TXMotorOn(TxMuxPort, 0,  0);  // brake
    TXMotorOn(TxMuxPort, 1,  0);  // brake
    start RefreshMotorEncValues;

    Wait(2000);

    stop RefreshMotorEncValues; Wait(50);
    TXMotorOn(TxMuxPort, 0, -50);
    TXMotorOn(TxMuxPort, 1, -20);
    start RefreshMotorEncValues;

    Wait(2000);

    stop RefreshMotorEncValues; Wait(50);
    TXMotorOn(TxMuxPort, 0,  0);    // brake
    TXMotorOn(TxMuxPort, 1, -128);  // coast
    start RefreshMotorEncValues;

    Wait(2000);

    stop RefreshMotorEncValues; Wait(50);
    ResetTXMotorCounter(TxMuxPort,0);
    ResetTXMotorCounter(TxMuxPort,1);
    start RefreshMotorEncValues;

    Wait(2000);


  }
}