BasicMicro - Forums

Serial1.begin(19200);

;Basic Micro Robo Claw Packet Serial Test Commands 0 to 13.
;Switch settings: SW3=ON and SW5=ON.
Main
Pause 2000
Serout P15, i19200, [128, 0, 127, (255 & 0x7F)];M1 full speed forward
Serout P15, i19200, [128, 4, 127, (259 & 0x7F)];M2 full speed forward

static byte s_c1[] = {128, 0, 127, 255 & 0x7f};
static byte s_c2[] = {128, 0, 127, 255 & 0x7f};
...
delay(2000);
Serial1.write(s_c1, sizeof(s_c1));
Serial1.write(s_c1, sizeof(s_c2));

void RC_DriveMotor1(byte bCon, byte bSpeed)
{
    byte ab[4];
    ab[0] = bCon;
    ab[1] = 0;
    ab[2] = bSpeed;
    ab[3] = (ab[0]+ab[1]+ab[2]) & 0x7f;
    Serial1.write(ab, sizeof(ab));
}

void setup()
{
  Serial.begin(9600);
}

void loop()
{
delay(5000);
  //RC_DriveMotor1(128, 127);
  //M1Speed(128, 6000, 6000);
  M1Dist(128, 36000, 36000, 36000);
delay(5000);
  //RC_DriveMotor1(128, 0);
  //M1Speed(128, 6000,-6000);
  //M1Speed(128, 6000,0);
  M1Dist(128, 36000, -36000, 36000);
}

void RC_DriveMotor1(byte bCon, byte bSpeed)
{
    byte ab[4];
    ab[0] = bCon;
    ab[1] = 0;
    ab[2] = bSpeed;
    ab[3] = (ab[0]+ab[1]+ab[2]) & 0x7f;
    Serial.write(ab, sizeof(ab));
}

void M1Speed(byte bCon, long Accel, long Speed) {
    byte ab[11];
    ab[0] = bCon;
    ab[1] = 38;
    ab[2] = (Accel >> 24) & 0xff;
    ab[3] = (Accel >> 16) & 0xff;
    ab[4] = (Accel >> 8) & 0xff;
    ab[5] = (Accel >> 0) & 0xff;
    ab[6] = (Speed >> 24) & 0xff;
    ab[7] = (Speed >> 16) & 0xff;
    ab[8] = (Speed >> 8) & 0xff;
    ab[9] = (Speed >> 0) & 0xff;
    ab[10] = (ab[0]+ab[1]+ab[2]+ab[3]+ab[4]+ab[5]+ab[6]+ab[7]+ab[8]+ab[9]) & 0x7f;
    Serial.write(ab, sizeof(ab));
}

void M1Dist(byte bCon, long Accel, long Speed, long Dist) {
    //Accel, Speed Dist
    byte ab[16];
    ab[0] = bCon;
    ab[1] = 44;
    ab[2] = (Accel >> 24) & 0xff;
    ab[3] = (Accel >> 16) & 0xff;
    ab[4] = (Accel >> 8) & 0xff;
    ab[5] = (Accel >> 0) & 0xff;
    ab[6] = (Speed >> 24) & 0xff;
    ab[7] = (Speed >> 16) & 0xff;
    ab[8] = (Speed >> 8) & 0xff;
    ab[9] = (Speed >> 0) & 0xff;
    ab[10] = (Dist >> 24) & 0xff;
    ab[11] = (Dist >> 16) & 0xff;
    ab[12] = (Dist >> 8) & 0xff;
    ab[13] = (Dist >> 0) & 0xff;
    ab[14] = 0; //buffer 0=yes, 1=no (immediate)
    ab[15] = (ab[0]+ab[1]+ab[2]+ab[3]+ab[4]+ab[5]+ab[6]+ab[7]+ab[8]+ab[9]+ab[10]+ab[11]+ab[12]+ab[13]+ab[14]) & 0x7f;
    Serial.write(ab, sizeof(ab));
}

#define RoboClawAddr 128    // the RoboClaw address
#define FullSpeed 36000L     // the top speed you want the motor to move in clicks per second
#define ClicksPerRev 12000L  // the number of encoder clicks per revolution

void setup()
{
  Serial.begin(9600);
}

void loop()
{
  delay(6000);
  M1TrapezoidDist(ClicksPerRev * 3);  // forward 3 revolutions
  delay(6000);
  M1TrapezoidDist(-ClicksPerRev * 3); // backward 3 revolutions
}

// A better approximation of the distance based on the comments in the Basic Micro forum
// This is not super accurate since the RobOClaw does PID on Speed, not Distance, but it's
// better than a pure Dist command
// The input is a signed distance in clicks
void M1TrapezoidDist(long tDist) {
  long decelDist;
  long accel;
  int speedSign;
 
  speedSign = tDist / abs(tDist);  // get the sign of the dist since the RoboClaw uses the sign on the speed param
  accel = FullSpeed; // assuming the Accel is the Same as speed (1 second to get to full speed)
  decelDist = (FullSpeed * FullSpeed) / (2 * accel);
  M1Dist(RoboClawAddr, accel, speedSign * FullSpeed, abs(tDist) - decelDist);
  M1Dist(RoboClawAddr, accel, 0, 0);  // buffered command will kick in once the distance above is hit, then drop to zero
}

void M1Dist(byte bCon, long Accel, long Speed, long Dist) {
    //Accel, Speed Dist
    byte ab[16];
    ab[0] = bCon;
    ab[1] = 44;
    ab[2] = (Accel >> 24) & 0xff;
    ab[3] = (Accel >> 16) & 0xff;
    ab[4] = (Accel >> 8) & 0xff;
    ab[5] = (Accel >> 0) & 0xff;
    ab[6] = (Speed >> 24) & 0xff;
    ab[7] = (Speed >> 16) & 0xff;
    ab[8] = (Speed >> 8) & 0xff;
    ab[9] = (Speed >> 0) & 0xff;
    ab[10] = (Dist >> 24) & 0xff;
    ab[11] = (Dist >> 16) & 0xff;
    ab[12] = (Dist >> 8) & 0xff;
    ab[13] = (Dist >> 0) & 0xff;
    ab[14] = 0; //buffer 0=yes, 1=no (immediate)
    ab[15] = (ab[0]+ab[1]+ab[2]+ab[3]+ab[4]+ab[5]+ab[6]+ab[7]+ab[8]+ab[9]+ab[10]+ab[11]+ab[12]+ab[13]+ab[14]) & 0x7f;
    Serial.write(ab, sizeof(ab));
}

#define address 128


/*
** 0 - Drive Forward M1
** Drive motor 1 forward. Valid data range is 0 - 127. A value of 127 = full speed forward, 64 = about
** half speed forward and 0 = full stop. Example with Robo Claw address set to 128:
**
** Serout P15, i19200, [128, 0, 127, (255 & 0X7F)] ;M1 full speed forward
*/

void drive_forward_m1(byte m1_speed_f)
{
   byte serial_data[4];
   serial_data[0]=address;
   serial_data[1]=0;
   serial_data[2]=m1_speed_f;
   serial_data[3]=(serial_data[0] + serial_data[1] + serial_data[2] + serial_data[3]) & 0x7F;
   
   serial.write(serial_data, sizeof(serial_data));
}


/*
** 1 - Drive Backwards M1
** Drive motor 1 backwards. Valid data range is 0 - 127. A value of 127 full speed backwards, 64 =
** about half speed backward and 0 = full stop. Example with Robo Claw address set to 128:
**
** Serout P15, i19200, [128, 1, 127, (256 & 0X7F)] ;M1 full speed backward
*/

void drive_backwards_m1(byte m1_speed_b)
{
   byte serial_data[4];
   serial_data[0]=address;
   serial_data[1]=1;
   serial_data[2]=m1_speed_b;
   serial_data[3]=(serial_data[0] + serial_data[1] + serial_data[2] + serial_data[3]) & 0x7F;
   
   serial.write(serial_data, sizeof(serial_data));
}


/*
** 2 - Set Minimum Main Voltage
** Sets main battery (B- / B+) minimum voltage level. If the battery voltages drops below the set
** voltage level Robo Claw will shut down. The value is cleared at start up and must set after each
** power up. The voltage is set in .2 volt increments. A value of 0 sets the minimum value allowed which
** is 6V. The valid data range is 0 - 120 (6V - 30V). The formula for calculating the voltage is: (Desired
** Volts - 6) x 5 = Value. Examples of valid values are 6V = 0, 8V = 10 and 11V = 25. Example with
** Robo Claw address set to 128:
**
** Serout P15, i19200, [128, 2, 25, (165 & 0X7F)]
*/

void min_voltage_value(byte min_battery_voltage)
{
   byte serial_data[4];
   serial_data[0]=address;
   serial_data[1]=2;
   serial_data[2]=min_battery_voltage;
   serial_data[3]=(serial_data[0] + serial_data[1] + serial_data[2]) & 0x7F;

   serial.write(serial_data, sizeof(serial_data));
}


/*
** 3 - Set Maximum Main Voltage
** Sets main battery (B- / B+) maximum voltage level. The valid data range is 0 - 154 (0V - 30V). If
** you are using a battery of any type you can ignore this setting. During regenerative breaking a back
** voltage is applied to charge the battery. When using an ATX type power supply if it senses anything
** over 16V it will shut down. By setting the maximum voltage level, Robo Claw before exceeding it will
** go into hard breaking mode until the voltage drops below the maximum value set. The formula for
** calculating the voltage is: Desired Volts x 5.12 = Value. Examples of valid values are 12V = 62, 16V
** = 82 and 24V = 123. Example with Robo Claw address set to 128:
**
** Serout P15, i19200, [128, 3, 82, (213 & 0X7F)]
*/

void max_voltage_value(byte max_battery_value)
{

   byte serial_data[4];
   serial_data[0]=address;
   serial_data[1]=3;
   serial_data[2]=min_battery_voltage;
   serial_data[3]=(serial_data[0] + serial_data[1] + serial_data[2]) & 0x7F;

   serial.write(serial_data, sizeof(serial_data));
}


/*
** 4 - Drive Forward M2
** Drive motor 2 forward. Valid data range is 0 - 127. A value of 127 = full speed forward, 64 = about
** half speed forward and 0 = full stop. Example with Robo Claw address set to 128:
**
** Serout P15, i19200, [128, 0, 127, (255 & 0X7F)] ;M2 full speed forward
*/

void drive_forward_m2(byte m2_speed_f)
{
   byte serial_data[4];
   serial_data[0]=address;
   serial_data[1]=4;
   serial_data[2]=m2_speed_f;
   serial_data[3]=(serial_data[0] + serial_data[1] + serial_data[2] + serial_data[3]) & 0x7F;
   
   serial.write(serial_data, sizeof(serial_data));
}


/*
** 5 - Drive Backwards M2
** Drive motor 2 backwards. Valid data range is 0 - 127. A value of 127 full speed backwards, 64 =
** about half speed backward and 0 = full stop. Example with Robo Claw address set to 128:
**
** Serout P15, i19200, [128, 1, 127, (256 & 0X7F)] ;M2 full speed backward
*/

void drive_backwards_m2(byte m2_speed_b)
{
   byte serial_data[4];
   serial_data[0]=address;
   serial_data[1]=5;
   serial_data[2]=m2_speed_b;
   serial_data[3]=(serial_data[0] + serial_data[1] + serial_data[2] + serial_data[3]) & 0x7F;
   
   serial.write(serial_data, sizeof(serial_data));
}


/*
** 6 - Drive M1 (7 Bit)
** Drive motor 1 forward and reverse. Valid data range is 0 - 127. A value of 0 = full speed reverse, 64 =
** stop and 127 = full speed forward. Example with Robo Claw address set to 128:
**
** Serout P15, i19200, [128, 6, 96, (230 & 0X7F)] ;M1 half speed forward
*/

void drive_m1(byte m1_speed)
{
   
   byte serial_data[4];
   serial_data[0]=address;
   serial_data[1]=6;
   serial_data[2]=m1_speed;
   serial_data[3]=(serial_data[0] + serial_data[1] + serial_data[2] + serial_data[3]) & 0x7F;
   
   serial.write(serial_data, sizeof(serial_data));
}

   
/*
** 7 - Drive M2 (7 Bit)
** Drive motor 2 forward and reverse. Valid data range is 0 - 127. A value of 0 = full speed reverse, 64 =
** stop and 127 = full speed forward. Example with Robo Claw address set to 128:
**
** Serout P15, i19200, [128, 7, 32, (167 & 0X7F)] ;M2 half speed reverse
*/

void drive_m2(byte m2_speed)
{

   byte serial_data[4];
   serial_data[0]=address;
   serial_data[1]=7;
   serial_data[2]=m2_speed_b;
   serial_data[3]=(serial_data[0] + serial_data[1] + serial_data[2] + serial_data[3]) & 0x7F;
   
   serial.write(serial_data, sizeof(serial_data));
}

void M1Speed(byte bCon, long Accel, long Speed) {
    byte ab[11];
    ab[0] = bCon;
    ab[1] = 38;
    ab[2] = (Accel >> 24) & 0xff;
    ab[3] = (Accel >> 16) & 0xff;
    ab[4] = (Accel >> 8) & 0xff;
    ab[5] = (Accel >> 0) & 0xff;
    ab[6] = (Speed >> 24) & 0xff;
    ab[7] = (Speed >> 16) & 0xff;
    ab[8] = (Speed >> 8) & 0xff;
    ab[9] = (Speed >> 0) & 0xff;
    ab[10] = (ab[0]+ab[1]+ab[2]+ab[3]+ab[4]+ab[5]+ab[6]+ab[7]+ab[8]+ab[9]) & 0x7f;
    Serial.write(ab, sizeof(ab));
}