BluefruitConfig.h

// COMMON SETTINGS
// ----------------------------------------------------------------------------------------------
// These settings are used in both SW UART, HW UART and SPI mode
// ----------------------------------------------------------------------------------------------
#define BUFSIZE                        100   // Size of the read buffer for incoming data
#define BLEBUFSIZE                     300   // Size of the read buffer for incoming data
#define VERBOSE_MODE                   true  // If set to 'true' enables debug output


// SOFTWARE UART SETTINGS
// ----------------------------------------------------------------------------------------------
// The following macros declare the pins that will be used for 'SW' serial.
// You should use this option if you are connecting the UART Friend to an UNO
// ----------------------------------------------------------------------------------------------
//#define BLUEFRUIT_SWUART_RXD_PIN       9    // Required for software serial!
//#define BLUEFRUIT_SWUART_TXD_PIN       10   // Required for software serial!
//#define BLUEFRUIT_UART_CTS_PIN         11   // Required for software serial!
//#define BLUEFRUIT_UART_RTS_PIN         -1   // Optional, set to -1 if unused


// HARDWARE UART SETTINGS
// ----------------------------------------------------------------------------------------------
// The following macros declare the HW serial port you are using. Uncomment
// this line if you are connecting the BLE to Leonardo/Micro or Flora
// ----------------------------------------------------------------------------------------------
#ifdef Serial1    // this makes it not complain on compilation if there's no Serial1
  #define BLUEFRUIT_HWSERIAL_NAME      Serial1
#endif


// SHARED UART SETTINGS
// ----------------------------------------------------------------------------------------------
// The following sets the optional Mode pin, its recommended but not required
// ----------------------------------------------------------------------------------------------
#define BLUEFRUIT_UART_MODE_PIN        12    // Set to -1 if unused


// SHARED SPI SETTINGS
// ----------------------------------------------------------------------------------------------
// The following macros declare the pins to use for HW and SW SPI communication.
// SCK, MISO and MOSI should be connected to the HW SPI pins on the Uno when
// using HW SPI.  This should be used with nRF51822 based Bluefruit LE modules
// that use SPI (Bluefruit LE SPI Friend).
// ----------------------------------------------------------------------------------------------
#define BLUEFRUIT_SPI_CS               8
#define BLUEFRUIT_SPI_IRQ              7
#define BLUEFRUIT_SPI_RST              4    // Optional but recommended, set to -1 if unused

// SOFTWARE SPI SETTINGS
// ----------------------------------------------------------------------------------------------
// The following macros declare the pins to use for SW SPI communication.
// This should be used with nRF51822 based Bluefruit LE modules that use SPI
// (Bluefruit LE SPI Friend).
// ----------------------------------------------------------------------------------------------
#define BLUEFRUIT_SPI_SCK              13
#define BLUEFRUIT_SPI_MISO             12
#define BLUEFRUIT_SPI_MOSI             11

Cmd.cpp

/*******************************************************************
    Copyright (C) 2009 FreakLabs
    All rights reserved.

    Redistribution and use in source and binary forms, with or without
    modification, are permitted provided that the following conditions
    are met:

    1. Redistributions of source code must retain the above copyright
       notice, this list of conditions and the following disclaimer.
    2. Redistributions in binary form must reproduce the above copyright
       notice, this list of conditions and the following disclaimer in the
       documentation and/or other materials provided with the distribution.
    3. Neither the name of the the copyright holder nor the names of its contributors
       may be used to endorse or promote products derived from this software
       without specific prior written permission.

    THIS SOFTWARE IS PROVIDED BY THE THE COPYRIGHT HOLDERS AND CONTRIBUTORS ``AS IS'' AND
    ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
    IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
    ARE DISCLAIMED.  IN NO EVENT SHALL THE INSTITUTE OR CONTRIBUTORS BE LIABLE
    FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
    DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
    OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
    HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
    OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
    SUCH DAMAGE.

    Originally written by Christopher Wang aka Akiba.
    Please post support questions to the FreakLabs forum.

*******************************************************************/
/*!
    \file Cmd.c

    This implements a simple command line interface for the Arduino so that
    its possible to execute individual functions within the sketch. 
*/
/**************************************************************************/
#include <avr/pgmspace.h>
#if ARDUINO >= 100
#include <Arduino.h>
#else
#include <WProgram.h>
#endif
#include "HardwareSerial.h"
#include "Cmd.h"

// command line message buffer and pointer
static uint8_t msg[MAX_MSG_SIZE];
static uint8_t *msg_ptr;

// linked list for command table
static cmd_t *cmd_tbl_list, *cmd_tbl;

// text strings for command prompt (stored in flash)
const char cmd_banner[] PROGMEM = "*************** CMD *******************";
const char cmd_prompt[] PROGMEM = "CMD >> ";
const char cmd_unrecog[] PROGMEM = "CMD: Command not recognized.";


/**************************************************************************/
/*!
    Parse the command line. This function tokenizes the command input, then
    searches for the command table entry associated with the commmand. Once found,
    it will jump to the corresponding function.
*/
/**************************************************************************/
void cmd_parse(char *cmd)
{
    uint8_t argc, i = 0;
    char *argv[30];
    char buf[50];
    cmd_t *cmd_entry;

    fflush(stdout);

    // parse the command line statement and break it up into space-delimited
    // strings. the array of strings will be saved in the argv array.
    argv[i] = strtok(cmd, " ");
    do
    {
        argv[++i] = strtok(NULL, " ");
    } while ((i < 30) && (argv[i] != NULL));
    
    // save off the number of arguments for the particular command.
    argc = i;

    // parse the command table for valid command. used argv[0] which is the
    // actual command name typed in at the prompt
    for (cmd_entry = cmd_tbl; cmd_entry != NULL; cmd_entry = cmd_entry->next)
    {
        if (!strcmp(argv[0], cmd_entry->cmd))
        {
            cmd_entry->func(argc, argv);
            Serial.print("Got you, sir: ");
            Serial.println(cmd);
            return;
        }
    }

    // command not recognized. print message and re-generate prompt.
    strcpy_P(buf, cmd_unrecog);
    Serial.println(buf);
}

/**************************************************************************/
/*!
    This function processes the individual characters typed into the command
    prompt. It saves them off into the message buffer unless its a "backspace"
    or "enter" key. 
*/
/**************************************************************************/
void cmd_handler()
{
    char c = Serial.read();

    switch (c)
    {
    case '\r':
        // terminate the msg and reset the msg ptr. then send
        // it to the handler for processing.
        *msg_ptr = '\0';
        Serial.print("\r\n");
        cmd_parse((char *)msg);
        msg_ptr = msg;
        break;
    
    case '\b':
        // backspace 
        Serial.print(c);
        if (msg_ptr > msg)
        {
            msg_ptr--;
        }
        break;
    
    default:
        // normal character entered. add it to the buffer
        Serial.print(c);
        *msg_ptr++ = c;
        break;
    }
}

/**************************************************************************/
/*!
    This function should be set inside the main loop. It needs to be called
    constantly to check if there is any available input at the command prompt.
*/
/**************************************************************************/
void cmdPoll()
{
    while (Serial.available())
    {
        cmd_handler();
    }
}

/**************************************************************************/
/*!
    Initialize the command line interface. This sets the terminal speed and
    and initializes things. 
*/
/**************************************************************************/
void cmdInit()
{
    // init the msg ptr
    msg_ptr = msg;

    // init the command table
    cmd_tbl_list = NULL;
}

/**************************************************************************/
/*!
    Add a command to the command table. The commands should be added in
    at the setup() portion of the sketch. 
*/
/**************************************************************************/
void cmdAdd(char *name, void (*func)(int argc, char **argv))
{
    // alloc memory for command struct
    cmd_tbl = (cmd_t *)malloc(sizeof(cmd_t));

    // alloc memory for command name
    char *cmd_name = (char *)malloc(strlen(name)+1);

    // copy command name
    strcpy(cmd_name, name);

    // terminate the command name
    cmd_name[strlen(name)] = '\0';

    // fill out structure
    cmd_tbl->cmd = cmd_name;
    cmd_tbl->func = func;
    cmd_tbl->next = cmd_tbl_list;
    cmd_tbl_list = cmd_tbl;
}

/**************************************************************************/
/*!
    Convert a string to a number. The base must be specified, ie: "32" is a
    different value in base 10 (decimal) and base 16 (hexadecimal).
*/
/**************************************************************************/
uint32_t cmdStr2Num(char *str, uint8_t base)
{
    return strtol(str, NULL, base);
}

Cmd.h

/*******************************************************************
    Copyright (C) 2009 FreakLabs
    All rights reserved.

    Redistribution and use in source and binary forms, with or without
    modification, are permitted provided that the following conditions
    are met:

    1. Redistributions of source code must retain the above copyright
       notice, this list of conditions and the following disclaimer.
    2. Redistributions in binary form must reproduce the above copyright
       notice, this list of conditions and the following disclaimer in the
       documentation and/or other materials provided with the distribution.
    3. Neither the name of the the copyright holder nor the names of its contributors
       may be used to endorse or promote products derived from this software
       without specific prior written permission.

    THIS SOFTWARE IS PROVIDED BY THE THE COPYRIGHT HOLDERS AND CONTRIBUTORS ``AS IS'' AND
    ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
    IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
    ARE DISCLAIMED.  IN NO EVENT SHALL THE INSTITUTE OR CONTRIBUTORS BE LIABLE
    FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
    DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
    OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
    HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
    OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
    SUCH DAMAGE.

    Originally written by Christopher Wang aka Akiba.
    Please post support questions to the FreakLabs forum.

*******************************************************************/
/*!
    \file 
    \ingroup


*/
/**************************************************************************/
#ifndef CMD_H
#define CMD_H

#define MAX_MSG_SIZE    60
#include <stdint.h>

// command line structure
typedef struct _cmd_t
{
    char *cmd;
    void (*func)(int argc, char **argv);
    struct _cmd_t *next;
} cmd_t;

void cmdInit();
void cmdPoll();
void cmdAdd(char *name, void (*func)(int argc, char **argv));
uint32_t cmdStr2Num(char *str, uint8_t base);

#endif //CMD_H

Messages.h

const unsigned MSG_PANCAKE = 0xfeba;
const unsigned MSG_SEGMENT = 0xfefa;

const int COORDS_CARTESIAN = 1;
const int COORDS_POLAR = 2;

struct Point {
  unsigned x;
  unsigned y;
};

struct PancakeMessage {
  unsigned coordSystem;
  unsigned segmentCount;
};

struct SegmentMessage {
  unsigned pointCount;
  unsigned leaveDown;
};

pancakes.ino

#include <Arduino.h>
#include <SPI.h>
#include "Adafruit_BLE.h"
#include "Adafruit_BluefruitLE_SPI.h"
#include "Adafruit_BluefruitLE_UART.h"

#include "BluefruitConfig.h"
#include "Messages.h"
#include "Cmd.h"

#if SOFTWARE_SERIAL_AVAILABLE
#include <SoftwareSerial.h>
#endif

#define FACTORYRESET_ENABLE         0
#define MINIMUM_FIRMWARE_VERSION    "0.6.6"
#define MODE_LED_BEHAVIOUR          "MODE"
#define SKIP_CALIBRATE              1

/* ...hardware SPI, using SCK/MOSI/MISO hardware SPI pins and then user selected CS/IRQ/RST */
Adafruit_BluefruitLE_SPI ble(BLUEFRUIT_SPI_CS, BLUEFRUIT_SPI_IRQ, BLUEFRUIT_SPI_RST);

/* ...software SPI, using SCK/MOSI/MISO user-defined SPI pins and then user selected CS/IRQ/RST */
//Adafruit_BluefruitLE_SPI ble(BLUEFRUIT_SPI_SCK, BLUEFRUIT_SPI_MISO,
//                             BLUEFRUIT_SPI_MOSI, BLUEFRUIT_SPI_CS,
//                             BLUEFRUIT_SPI_IRQ, BLUEFRUIT_SPI_RST);

const int irPin1 = 2;
const int irPin2 = 3;
const int dirPinA = 9;
const int pwmPinA = 10;
const int dirPinB = 17;
const int stepPinB = 16;
const int sleepPinB = 15;
const int pwmPump = 5;
const int dirPump = 6;
const int sensePin = 4;
const float voltPerAmper = 1.65;
const float maxMoverPower = 0.40;

const int minValidPos = 230;
const int maxValidPos = 1684;
const int halfCircleRotation = 220;
const int maxPosition = 1982;
const int centerPosition = maxPosition / 2;

void (*doneMoving)();
void (*doneRotating)();

bool pumping = false;
bool moving = false;
bool rotating = false;
int movingSpeed = 200;
int rotationSpeed = 100;
int maxMovingSpeed = 210;
int movingTarget;
int rotationTarget;
int currentPosition = 0;
int currentRotation = 0;
unsigned long lastRotationStep = 0;

void error(const char *err) {
  Serial.println(err);
  while (1);
}

void setup(void)
{
  Serial.begin(115200);

  Serial.println("PAAAAAAAAAAAAAANNNNNCCAAAAAAAAAAAAAAAAKKKERRRRRR");
  Serial.println("------------------------------------------------");

  pinMode(dirPinA, OUTPUT);
  pinMode(pwmPinA, OUTPUT);
  pinMode(stepPinB, OUTPUT);
  pinMode(dirPinB, OUTPUT);
  pinMode(sleepPinB, OUTPUT);
  pinMode(pwmPump, OUTPUT);
  pinMode(dirPump, OUTPUT);
  pinMode(irPin1, INPUT_PULLUP);
  pinMode(irPin2, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(irPin1), positionInterrupt, RISING);

  calibrateA();

  Serial.print("Initialising the Bluefruit LE module: ");

  if (!ble.begin(VERBOSE_MODE)) {
    error("Couldn't find Bluefruit, make sure it's in CoMmanD mode & check wiring?");
  }
  Serial.println("OK!");

  if (FACTORYRESET_ENABLE) {
    Serial.println("Performing a factory reset: ");
    if (!ble.factoryReset()) {
      error("Couldn't factory reset");
    }
  }

  ble.echo(false);

  Serial.println("Requesting Bluefruit info:");
  ble.info();

  ble.verbose(false);

//  Serial.println("Waiting for connection...");
//  while (!ble.isConnected()) {
//    delay(500);
//  }

  if (ble.isVersionAtLeast(MINIMUM_FIRMWARE_VERSION)) {
    ble.sendCommandCheckOK("AT+HWModeLED=" MODE_LED_BEHAVIOUR);
  }

  ble.setTimeout(10000);
  ble.setMode(BLUEFRUIT_MODE_DATA);
  Serial.println("We are ready to PANCAKE");
  
  cmdInit();
//  cmdAdd("echo", handleEcho);
//  cmdAdd("cal", handleCalibrate);
  cmdAdd("rot", handleRotateDelta);
  cmdAdd("pos", handleMoveDelta);
  cmdAdd("pmp", handlePump);
  cmdAdd("fill", handleFill);
  cmdAdd("stop", handleStop);
}

//void handleCalibrate(int argc, char **argv) {
//  calibrateA();
//  calibrateB();
//}
//
//void handleEcho(int argc, char **argv) {
//  Serial.print("Heey ");
//  for (int i = 0; i < argc; i++) {
//    Serial.print(argv[i]);
//    Serial.print(", ");
//  }
//  Serial.println();
//}

void pumpItUp() {
  moveTo(0);
  analogWrite(pwmPump, 255);
  delay(1000);
  analogWrite(pwmPump, 0);
  moveTo(minValidPos);
  analogWrite(pwmPump, 255);
}

bool safeMode = false;
bool rotateLeft = true;
void handleFill(int argc, char **argv) {
  safeMode = argc > 1;
  if (!safeMode) {
    pumpItUp();
    pumping = true;
  }
  doneMoving = fillDoneMoving;
  doneRotating = fillDoneRotating;
  rotationTarget = -halfCircleRotation * 2;
  rotationSpeed = 10;
  rotating = true;
  movingTarget = maxValidPos;
  movingSpeed = 200;
  moving = true;
}
void fillDoneRotating() {
  rotateLeft = !rotateLeft;
  rotationTarget = rotateLeft ? -halfCircleRotation * 2 : 0;
  rotationSpeed = 10;
  rotating = true;
}
void fillDoneMoving() {
  doneMoving = 0;
  doneRotating = 0;
  pumping = false;
  Serial.println("You are all filled now");
  movingTarget = 0;
  movingSpeed = 255;
  moving = true;
}

void handleRotateDelta(int argc, char **argv) {
  if (argc == 1) {
    Serial.print(rotating ? "YE, " : "NO, ");
    Serial.print(currentRotation);
    Serial.print(" -> ");
    Serial.print(rotationTarget);
    Serial.print(", ");
    Serial.println(rotationSpeed);
    return;
  }
  if (argc > 1) {
    rotationTarget = cmdStr2Num(argv[1], 10);
    rotationSpeed = 100;
    rotating = true;
  }
  if (argc > 2) {
    rotationSpeed = cmdStr2Num(argv[2], 10);
  }
}

void handleMoveDelta(int argc, char **argv) {
  if (argc == 1) {
    Serial.print(moving ? "YE, " : "NO, ");
    Serial.print(currentPosition);
    Serial.print(" -> ");
    Serial.print(movingTarget);
    Serial.print(", ");
    Serial.println(movingSpeed);
    Serial.println(getMovingSpeed());
    return;
  }
  if (argc > 1) {
    movingTarget = cmdStr2Num(argv[1], 10);
    movingSpeed = 255;
    moving = true;
  }
  if (argc > 2) {
    movingSpeed = cmdStr2Num(argv[2], 10);
  }
}

void handlePump(int argc, char **argv) {
  if (argc > 1 && strcmp(argv[1], "on") == 0) {
    pumping = true;
  }
  if (argc > 1 && strcmp(argv[1], "off") == 0) {
    pumping = false;
  }
  if (argc == 1) {
    pumping = !pumping;
  }
  analogWrite(pwmPump, pumping ? 255 : 0);
}

void handleStop() {
  moving = false;
  rotating = false;
  pumping = false;
}

int sign(int v) {
  if (v == 0) {
    return 0;
  }
  return v > 0 ? 1 : -1;
}

float getMovingSpeed() {
  float coeff = 1.f - (float)abs(centerPosition - currentPosition) / centerPosition;
  return (float)movingSpeed + coeff * (maxMovingSpeed - movingSpeed);
}

void loop(void) {
  digitalWrite(dirPinA, (currentPosition < movingTarget) ? LOW : HIGH);
  digitalWrite(dirPinB, (currentRotation < rotationTarget) ? HIGH : LOW);
  digitalWrite(sleepPinB, rotating ? HIGH : LOW);

  analogWrite(pwmPump, pumping ? 255 : 0);
  analogWrite(pwmPinA, moving ? getMovingSpeed() : 0);

  if (moving && digitalRead(dirPinA) == LOW && currentPosition > movingTarget - 10) {
    movingSpeed = 0;
    moving = false;
    if (doneMoving) {
      doneMoving();
    }
  }
  if (moving && digitalRead(dirPinA) == HIGH && currentPosition < movingTarget + 10) {
    movingSpeed = 0;
    moving = false;
    if (doneMoving) {
      doneMoving();
    }
  }
  
  if (rotating && millis() - lastRotationStep > rotationSpeed) {
    lastRotationStep = millis();
    digitalWrite(stepPinB, HIGH);
    delayMicroseconds(500);
    digitalWrite(stepPinB, LOW);
    delayMicroseconds(500);
    currentRotation += digitalRead(dirPinB) ? 1 : -1;
  }

  if (rotating && digitalRead(dirPinB) == HIGH && currentRotation > rotationTarget) {
    rotationSpeed = 0;
    rotating = false;
    if (doneRotating) {
      doneRotating();
    }
  }
  if (rotating && digitalRead(dirPinB) == LOW && currentRotation < rotationTarget) {
    rotationSpeed = 0;
    rotating = false;
    if (doneRotating) {
      doneRotating();
    }
  }

  cmdPoll();

  char bleBuf[BLEBUFSIZE + 1];
  if (ble.available()) {
    Serial.println("Available ble bytes:");
    Serial.println(ble.available());
    
    int length = ble.readBytes(bleBuf, 2);
    if (length < 2) {
      Serial.println("Fatal: message reading timeout [1]");
      ble.println("FUCK [0xBAC0]");
      return;
    }

    unsigned msgType = *(unsigned*)bleBuf;
    Serial.print("Got something: ");
    Serial.println(msgType, HEX);
    switch (msgType) {
      case MSG_PANCAKE: {
        int length = ble.readBytes(bleBuf, sizeof(PancakeMessage));
        if (length < sizeof(PancakeMessage)) {
          Serial.println("Fatal: message reading timeout [2]");
          ble.println("FUCK [0xBAC1]");
          return;
        }
        PancakeMessage* msg = (PancakeMessage*)bleBuf;
        Serial.print("Got pancake message: ");
        Serial.print(msg->coordSystem);
        Serial.print(", ");
        Serial.println(msg->segmentCount);
        delay(1000);
        ble.println("ACK");
        break;
      }
      case MSG_SEGMENT: {
        int length = ble.readBytes(bleBuf, sizeof(SegmentMessage));
        if (length < sizeof(SegmentMessage)) {
          Serial.println("Fatal: message reading timeout [3]");
          ble.println("FUCK [0xBAC2]");
          return;
        }
        SegmentMessage* msg = (SegmentMessage*)bleBuf;
        Serial.print("Got segment message: ");
        Serial.print(msg->pointCount);
        Serial.print(", ");
        Serial.println(msg->leaveDown);
        length = ble.readBytes(bleBuf + sizeof(SegmentMessage), msg->pointCount * sizeof(Point));
        if (length < msg->pointCount * sizeof(Point)) {
          Serial.println("Fatal: message reading timeout [4]");
          ble.println("FUCK [0xBAC3]");
          return;
        }
        Serial.println("Received all points, proceeding to pancaking");
        delay(1000);
        ble.println("ACK");
        Point *points = (Point*)(bleBuf + sizeof(SegmentMessage));
        letsDrawSomePancakes(msg, points);
        break;
      }
      default: {
        Serial.println("Ignoring unknown message");
        ble.println("FUCK [0xBAAA]");
        break;
      }
    }
  }
}

void positionInterrupt() {
  int delta = digitalRead(irPin2) ? 1 : -1;
  currentPosition += delta;
}

float measureCurrent() {
  float currentRaw = analogRead(sensePin);
  float currentVolts = currentRaw * (5.0 / 1024.0);
  return currentVolts / voltPerAmper;
}

void moveUntilWeCan() {
  analogWrite(pwmPinA, 255);
  int maxCurrentTicks = 0;
  int start = millis();
  while (true) {
    if (measureCurrent() > maxMoverPower) {
      maxCurrentTicks++;
    } else {
      maxCurrentTicks = 0;
    }
    if (maxCurrentTicks > 1000) {
      analogWrite(pwmPinA, 0);
      return;
    }
    if (millis() - start > 5000) {
      Serial.println("Pass failed");
      analogWrite(pwmPinA, 0);
      return;
    }
  }
}

void calibrateA() {
  Serial.println("Calibrating The Mover...");
  digitalWrite(dirPinA, LOW);
  moveUntilWeCan();
  digitalWrite(dirPinA, HIGH);
  moveUntilWeCan();
  Serial.println("Done?!");
  currentPosition = 0;
  moveTo(minValidPos);
}

void calibrateB() {
  currentRotation = 0;
}

void calibratePump() {
  analogWrite(pwmPump, 255);
  delay(2000);
  analogWrite(pwmPump, 0);
}

void moveTo(int pos) {
  int direction = pos > currentPosition ? 1 : -1;
  Serial.println("Moving!");
  digitalWrite(dirPinA, direction > 0 ? LOW : HIGH);
  analogWrite(pwmPinA, 255);
  int maxCurrentTicks = 0;
  while (true) {
    if (measureCurrent() > maxMoverPower) {
      maxCurrentTicks++;
    } else {
      maxCurrentTicks = 0;
    }
    if (maxCurrentTicks > 1000) {
      Serial.println("Fatal: too much power to Move");
      analogWrite(pwmPinA, 0);
      return;
    }
    if ((currentPosition - pos) * direction >= 0) {
      Serial.println("Done!?");
      analogWrite(pwmPinA, 0);
      return;
    }
  }
}

void letsDrawSomePancakes(SegmentMessage *msg, Point *points) {
  for (int i = 0; i < msg->pointCount; i++) {
    float x = (float)points[i].x / 0xFFFF * maxPosition;
    Serial.print("Procceeding to ");
    Serial.println(x);
    moveTo((int)x);
    delay(1000);
  }
  Serial.println("Done drawing... Ahha no, never, you would never finish this project in time");
}