Mindboards

Code: Select all

/*
*  Matthew Richardson
*  matthewrichardson37<at>gmail.com
*  http://mattallen37.wordpress.com/
*  Aug. 11 / 2012
*
*  You may use this code as you wish, provided you give credit where it's due.
*
*  This is library for easy use of RS485 master/slave transactions.
*/

/*
My RS485 protocol data specs:

Sending byte array looks like this (based on how Mark Crosby does it) :
  
byte   type         description
0      MSG_TYPE     The type of message.
1      BYTE_COUNT   The count of bytes in the message body, excluding the header.
2      CHECKSUM     CRC8 checksum computed across the message body.
3      DEST_ADDR    Destination address for the message, or 0 for BROADCAST.
4      SRC_ADDR     Source address of the message sender.
5-n    The data

Returned values

-6 wrong message length
-5 wrong checksum
-4 not even the entire header was received
-3 not my address
-2 timeout
-1 something went wrong
0  Destination address was BROADCAST
1  Destination address was mine

*/

#ifndef _MyRS485Base_h_
#define _MyRS485Base_h_

void MyRS485BaseWriteArray (byte My485Address, byte TargetAddress, byte MsgType, byte SendData[], byte Length = 0){

  byte ByteCount = ArrayLen(SendData);

  if(Length){
    ByteCount = Length;
  }

  byte SendCompleteData[];
  byte SendCompleteDataLength = ByteCount + 5;
  ArrayInit(SendCompleteData, 0, SendCompleteDataLength);

  SendCompleteData[0] = MsgType;
  SendCompleteData[1] = ByteCount;
  SendCompleteData[2] = ArraySum(SendData, NA, ByteCount) & 0xFF;
  SendCompleteData[3] = TargetAddress;
  SendCompleteData[4] = My485Address;
  
  for(byte i = 0; i < ByteCount; i++){
    SendCompleteData[i + 5] = SendData[i];
  }

  RS485Write(SendCompleteData);
  until(HSOutputBufferOutPtr() == SendCompleteDataLength);
}

int MyRS485BaseRead(byte My485Address, byte & MsgType, byte & InArray[], int timeout = 250){
  byte RawInArray[];

  long OrigionalTick = CurrentTick();
  until(RS485DataAvailable()){                                                  // Wait until data has been received
    if(timeout && (CurrentTick() - OrigionalTick >= timeout))return -2;         // return -2 if it timed-out waiting for the responce.
    Yield();
  }
  byte DataAvailable = RS485DataAvailable();
  Wait(1);
  while(DataAvailable < RS485DataAvailable()){                                  // If it's been 1 ms since the last data was received, assume it's the end of the message.
    DataAvailable = RS485DataAvailable();
    Wait(1);
  }
  RS485Read(RawInArray);

  if(DataAvailable < 5)return -4;                                               // Not even the entire header was received.
  
  MsgType        = RawInArray[0];
  byte ByteCount = RawInArray[1];
  byte Checksum  = RawInArray[2];
  byte DestAddr  = RawInArray[3];
  byte SrcAddr   = RawInArray[4];

  if (DestAddr == My485Address || DestAddr == 0)
  {

    if(ByteCount != DataAvailable - 5){
      return -6;
    }

    byte ChecksumComputed = ArraySum(RawInArray, 5, NA) & 0xFF;
    if(Checksum != ChecksumComputed){
      return -5;
    }
    
    ArraySubset(InArray, RawInArray, 5, NA);
    
    return DestAddr?1:0;
  }
  else{
    return -3;
  }
  return -1;
}

void RS485Init(byte speed = HS_BAUD_921600)      // Turn on RS485 and set a baud rate
{
  UseRS485();
  Wait(MS_10);
  SetHSDataMode(DATA_MODE_RAW);
  Wait(MS_10);
  RS485Uart(speed, HS_MODE_DEFAULT);
  Wait(MS_10);
  SetHSInputBufferInPtr(0);
  SetHSInputBufferOutPtr(0);
}

void RS485UnInit(){                              // Turn off the RS485 port of the NXT
  SetSensorType(S4, SENSOR_TYPE_NONE);
  SetHSFlags(HS_UPDATE);
  SetSensorType(S4, SENSOR_TYPE_NONE);
}

#endif

Code: Select all

#include "MyRS485Base lib.nxc"

byte Array[];
byte MsgType;

task main(){

  RS485Init(HS_BAUD_115200);

  while(true){
    //NumOut(0, LCD_LINE5, MyRS485BaseRead(2, MsgType, Array, 100));
    int result = MyRS485BaseRead(2, MsgType, Array, 100);
    ClearScreen();
    NumOut(0, LCD_LINE1, result);
    if(result >= 0){
      NumOut(0, LCD_LINE2, MsgType);
      for(byte i = 0; i < ArrayLen(Array); i ++){
        NumOut(50, LCD_LINE1 - (i*8), Array[i]);
      }
    }
  }
}

Code: Select all

#include "MyRS485Base lib.nxc"

byte Array[6];
byte MsgType;
byte b1, b2, lx, ly, rx, ry;

task main(){

  RS485Init(HS_BAUD_115200);

  SetSensorLowspeed(S3);
  
  while(true){
  
    ReadSensorMSPlayStation(S3, 0x02, b1, b2, lx, ly, rx, ry);
    Array[0] = b1;
    Array[1] = b2;
    Array[2] = lx;
    Array[3] = ly;
    Array[4] = rx;
    Array[5] = ry;

    MsgType = ButtonPressed(BTNCENTER)?2:0;

    MyRS485BaseWriteArray(1, 2, MsgType, Array);
    
    Wait(10);
  }
}

Code: Select all

//------------------------------------------------------------------------------
char CmdRdy;

//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
inline void MotorOff(const byte &port)
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
{
   SetOutput(port, Power, 0,
             OutputMode,  OUT_MODE_COAST,
             RegMode,     OUT_REGMODE_IDLE,
             RunState,    OUT_RUNSTATE_IDLE,
             UpdateFlags, UF_UPDATE_MODE + UF_UPDATE_SPEED);
}


//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
task Claw_up() {
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   CmdRdy=false;
   OUT_MSMux(RCXMMux_Port, OUT_Claw, 1024);
   Wait(1);
   while(!(SensorValue(S3))) {  // dn pos = S3
     OUT_MSMux(RCXMMux_Port, OUT_Claw, -100);
     Wait(1);
     if (SensorValue(S3)) {
        OUT_MSMux(RCXMMux_Port, OUT_Claw, 1024);
        Wait(50);
     }
  }
  OUT_MSMux(RCXMMux_Port, OUT_Claw, 0);
  CmdRdy=true;
  Wait(1);
}


//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
task Claw_md() {
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  char speed ;
  speed=100;
  CmdRdy=false;

  OUT_MSMux(RCXMMux_Port, OUT_Claw, 1024);
  Wait(1);
  while(!(SensorValue(S2))) {
      if(SensorValue(S1)) speed=-100;
      OUT_MSMux(RCXMMux_Port, OUT_Claw, speed);
      Wait(1);
      if (SensorValue(S2)) {  // dn pos = S1
        OUT_MSMux(RCXMMux_Port, OUT_Claw, 1024);
        speed=0;
        Wait(50);
      }
  }
  OUT_MSMux(RCXMMux_Port, OUT_Claw, 0);
  CmdRdy=true;
  Wait(1);
}

//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
task Claw_dn() {
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  CmdRdy=false;
  OUT_MSMux(RCXMMux_Port, OUT_Claw, 1024);
  Wait(1);
  while(!(SensorValue(S1))) {
      OUT_MSMux(RCXMMux_Port, OUT_Claw, 100);
      Wait(1);
      if (SensorValue(S1)) {  // dn pos = S1
        MotorOff(OUT_Claw);
        Wait(50);
      }
  }
  OUT_MSMux(RCXMMux_Port, OUT_Claw, 0);
  CmdRdy=true;
  Wait(1);
}