NXC: Tetrix Motor Controller Treiber

Moderator: Moderatoren

Benutzeravatar
HaWe
Administrator
Administrator
Beiträge: 5399
Registriert: 11. Jan 2006 21:01
Wohnort: ein kleiner Planet in der Nähe von Beteigeuze

NXC: Tetrix Motor Controller Treiber

Beitragvon HaWe » 5. Dez 2010 14:36

Ein Versuch:
NXC Tetrix Motor Controller Treiber

Bitte testet mal, ob was passiert (vllt ist mein Controller defekt):

Code: Alles auswählen

// edit: neuer Code s.u.!
Zuletzt geändert von HaWe am 2. Jan 2011 14:36, insgesamt 2-mal geändert.
Gruß,
HaWe
±·≠≈²³αβγδε∂ζλμνπξφωΔΦ≡ΠΣΨΩ∫√∀∃∈∉∧∨¬⊂⊄∩∪∅∞®
NXT NXC SCHACHROBOTER: https://www.youtube.com/watch?v=Cv-yzuebC7E

Benutzeravatar
HaWe
Administrator
Administrator
Beiträge: 5399
Registriert: 11. Jan 2006 21:01
Wohnort: ein kleiner Planet in der Nähe von Beteigeuze

Beitragvon HaWe » 31. Dez 2010 14:38

funktoniert (halbwegs)!
Tetrix Motor Treiber für NXC

TXMotorOn schaltet wie OnFwd,

Code: Alles auswählen

void TXMotorOn(byte NXTport, byte TXmotor, char percentage)


1. Übergabe-Parameter= NXTSensor(!) Port, an dem der Controller hängt
2. = die Nummer als Tetrix-Motor (zählt ab 0=Null)
3. = percentage = %PWM zwischen -100 bis +100, 0=stop/break, -128=float/coast
neu: auch die Encoder-Werte werden jetzt angezeigt und können auch resettet werden!

Code: Alles auswählen

// program Tetrix_Motor_Driver ver. 0.14

#define printf5( _x, _y, _format1,_format2,_format3,_format4,_format5,_value1,_value2,_value3,_value4,_value5) { \
  string sval1 = FormatNum(_format1, _value1); \
  string sval2 = FormatNum(_format2, _value2); \
  string sval3 = FormatNum(_format3, _value3); \
  string sval4 = FormatNum(_format4, _value4); \
  string sval5 = FormatNum(_format5, _value5); \
  string s =sval1+sval2+sval3+sval4+sval5; \
  TextOut(_x, _y, s); \
}

#define printf2( _x, _y, _format1, _format2, _value1, _value2) { \
  string sval1 = FormatNum(_format1, _value1); \
  string sval2 = FormatNum(_format2, _value2); \
  string s =sval1+sval2; \
  TextOut(_x, _y, s); \
}

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

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(1);
  IOresult=I2CWrite(NXTport, retLen, modeMsg);
  if (IOresult != NO_ERR)
    { printf5(0,48-(8*TXmotor),"%d","%3d","%2d","%5d","%4d",TXmotor,modeMsg[1],modeMsg[2],powerMsg[2],IOresult);
      return;  }
  printf5(0,48-(8*TXmotor),"%d","%3d","%2d","%5d","%4d",TXmotor,modeMsg[1],modeMsg[2],powerMsg[2],IOresult);
  Wait(1);

  // Send the second message when the first one is done
  ArrayBuild(powerMsg, devAddr, powerReg, percentage);
  while ((I2CCheckStatus(NXTport) == STAT_COMM_PENDING) && (I2CCheckStatus(NXTport) != NO_ERR)) Wait(1);
  IOresult=I2CWrite(NXTport, retLen, powerMsg);
  if (IOresult != NO_ERR)
    { printf5(0,48-(8*TXmotor),"%d","%3d","%2d","%5d","%4d",TXmotor,modeMsg[1],modeMsg[2],powerMsg[2],IOresult);
    return;  }

  printf5(0,48-(8*TXmotor),"%d","%3d","%2d","%5d","%4d",TXmotor,modeMsg[1],modeMsg[2],powerMsg[2],IOresult);

  Wait(1);
}

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

}

long GetTXMotorCounter(byte NXTport, byte TXmotor)
{
  byte devAddr;
  const byte msgLen = 4;
  byte sendMsg[];
  byte replyMsg[];
  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)) {
     return replyMsg[3] + (replyMsg[2] << 8) + (replyMsg[1] << 16) + (replyMsg[0] << 24);
  }

}


task showValues()  {
  long encoder0, encoder1;
  while(1) {
    encoder0= GetTXMotorCounter(0, 0);
    encoder1= GetTXMotorCounter(0, 1);
    Wait(10);
    printf1(0,8,"M1=%7d", encoder0);
    printf1(0,0,"M2=%7d", encoder1);
  }
}

task main(){

  TextOut(0,56,"M Rg md pow error");
  SetSensorType(S1, SENSOR_TYPE_LOWSPEED);
  Wait(20);
  ResetSensor(S1);
  Wait(50);
  StartTask(showValues);

  while (true) {                // test loop
 
    TXMotorOn(0, 0, 50);
    TXMotorOn(0, 1, 20);
    Wait(2000);

    TXMotorOn(0, 0,  0);  // brake
    TXMotorOn(0, 1,  0);  // brake
    Wait(2000);

    TXMotorOn(0, 0, -50);
    TXMotorOn(0, 1, -20);
    Wait(2000);

    TXMotorOn(0, 0,  0);   // brake
    TXMotorOn(0, 1, -128);   // coast
    Wait(2000);

    ResetTXMotorCounter(0,0);
    ResetTXMotorCounter(0,1);
    Wait(2000);
  }
}
Zuletzt geändert von HaWe am 2. Jan 2011 14:38, insgesamt 3-mal geändert.
Gruß,
HaWe
±·≠≈²³αβγδε∂ζλμνπξφωΔΦ≡ΠΣΨΩ∫√∀∃∈∉∧∨¬⊂⊄∩∪∅∞®
NXT NXC SCHACHROBOTER: https://www.youtube.com/watch?v=Cv-yzuebC7E

Benutzeravatar
HaWe
Administrator
Administrator
Beiträge: 5399
Registriert: 11. Jan 2006 21:01
Wohnort: ein kleiner Planet in der Nähe von Beteigeuze

Beitragvon HaWe » 2. Jan 2011 11:52

nochmal meine dringende Bitte an alle, die einen solchen Tetrix Motor Controller haben:
Bitte testet mal den Code mit der Standard- oder der enhanced Firmware und sagt mir, ob alle Motor-Kommandos richtig ausgeführt werden oder ob Ihr Aussetzer zwischendrin habt (z.B. dass nur 1 Motor läuft, obwohl immer beide gleichzeitig laufen müssten)!

Danke!
Zuletzt geändert von HaWe am 5. Jan 2011 19:45, insgesamt 1-mal geändert.
Gruß,
HaWe
±·≠≈²³αβγδε∂ζλμνπξφωΔΦ≡ΠΣΨΩ∫√∀∃∈∉∧∨¬⊂⊄∩∪∅∞®
NXT NXC SCHACHROBOTER: https://www.youtube.com/watch?v=Cv-yzuebC7E

Benutzeravatar
HaWe
Administrator
Administrator
Beiträge: 5399
Registriert: 11. Jan 2006 21:01
Wohnort: ein kleiner Planet in der Nähe von Beteigeuze

Beitragvon HaWe » 5. Jan 2011 19:24

hallo, jemand zuhause?
Gruß,
HaWe
±·≠≈²³αβγδε∂ζλμνπξφωΔΦ≡ΠΣΨΩ∫√∀∃∈∉∧∨¬⊂⊄∩∪∅∞®
NXT NXC SCHACHROBOTER: https://www.youtube.com/watch?v=Cv-yzuebC7E


Zurück zu „Tetrix, Lego Technik, Wedo ...“

Wer ist online?

Mitglieder in diesem Forum: 0 Mitglieder und 6 Gäste

Lego Mindstorms EV3, NXT und RCX Forum : Haftungsauschluss