diff --git a/examples/BasicRobot/ArcadeDrive.ino b/examples/BasicRobot/ArcadeDrive.ino new file mode 100644 index 0000000..bff88c8 --- /dev/null +++ b/examples/BasicRobot/ArcadeDrive.ino @@ -0,0 +1,35 @@ +#include "Gizmo.h" + +#define MAXMAG 90 + +int rampStep = 0; + +int SoftenMagnitude(int mag) { + + if (abs(mag) < 1) { + return 0; //center deadzone + } + + return (mag<0 ? -1:1 )* log(abs(mag)) * MAXMAG / log(MAXMAG); + float adj = (mag * mag) / MAXMAG; + return (mag < 0 ? 1 : -1 )* (int)adj; +} + +//Ramp up across .75 seconds +#define MAXRAMP 15 + +void ArcadeTarget(int XAXIS, int YAXIS, int &targetL, int &targetR) { + int x = SoftenMagnitude(map(gizmo.getAxis(XAXIS), 0, 255, -90, 90)); + int y = SoftenMagnitude(map(gizmo.getAxis(YAXIS), 0, 255, -90, 90)); + + if (abs(y)>30) x=x/2; + + if (abs(x)+abs(y)<20) { + rampStep = 1; + } else { + rampStep = MIN(rampStep+1, MAXRAMP); + } + + targetR = (MAX(MIN(y + x,90),-90)*rampStep/MAXRAMP) + MAXMAG; + targetL = (MIN(MAX(y - x,-90),90)*rampStep/MAXRAMP) + MAXMAG; +} diff --git a/examples/BasicRobot/BasicRobot.ino b/examples/BasicRobot/BasicRobot.ino index 6525077..73175ad 100644 --- a/examples/BasicRobot/BasicRobot.ino +++ b/examples/BasicRobot/BasicRobot.ino @@ -1,30 +1,61 @@ #include #include "Gizmo.h" +// The Gizmo provides access to the data that is held by the field +// management system and the gizmo system processor. +Gizmo gizmo; + +#include "PositionalServo.h" +#include "StatefulButton.h" +//#include "Tasks.h" //Un-comment to make use tasks for splitting up code + // Define a left and right drive motor. Connected via MC-29 motor // controllers they appear as servos to the system. Servo DriveL, DriveR; -// The Gizmo provides access to the data that is held by the field -// management system and the gizmo system processor. -Gizmo gizmo; +// Define a motor to sue for the arm +Servo Arm; + +// Define a claw using a positional servo. This will keep the servo in a posotion as its controls are used +PositionalServo Claw; + +//Create a Button to toggle into and out of Arcade Mode +StatefulButton BtnStart; + +//By default, start wth Arcade mode disabled +bool bArcadeMode = false; // Initialize the hardware and configure libraries for use. void setup() { // Initialize the connection to the system processor. gizmo.begin(); - // Configure the I/O mode for the motor driver pins. + // Configure the I/O mode for the motor driver pins and claw pinMode(GIZMO_MOTOR_1, OUTPUT); - pinMode(GIZMO_MOTOR_2, OUTPUT); + pinMode(GIZMO_MOTOR_8, OUTPUT); + pinMode(GIZMO_MOTOR_4, OUTPUT); + pinMode(GIZMO_MOTOR_5, OUTPUT); // Configure the builtin LED so we can tell the program is running. pinMode(LED_BUILTIN, OUTPUT); // Attach the motor controller objects to the ports the motors are // connected to. - DriveL.attach(GIZMO_MOTOR_1); - DriveR.attach(GIZMO_MOTOR_2); + DriveR.attach(GIZMO_MOTOR_1); + DriveL.attach(GIZMO_MOTOR_8); + Arm.attach(GIZMO_MOTOR_5); + + //Initialize the position claw on port 4 using left trigger and bumber buttons + Claw.init(GIZMO_MOTOR_4, GIZMO_BUTTON_LT, GIZMO_BUTTON_LSHOULDER, PS_SPEED_ULTRA); + + + //Use serial for Debug output + Serial.begin(9600); + + //Give serial startup a chance to get going + delay(500); + + Serial.println("BASIC ROBOT READY TO GO"); } // Runs in a loop to perform tasks that are required to run the robot. @@ -36,21 +67,42 @@ void loop() { // Refreshes the information about axis and button state. gizmo.refresh(); - // Fetch the speed of each axis, then convert this to the range - // expected by the motors. + //If start button gets clicked, toggle Arcade Mode + if (BtnStart.clicked()) { + bArcadeMode = !bArcadeMode; + Serial.printf("Drive Mode is now %s\r\n", bArcadeMode ? "ARCADE" : "TANK"); + } + int targetL, targetR; - targetL = map(gizmo.getAxis(GIZMO_AXIS_LY), 0, 255, 0, 180); - targetR = map(gizmo.getAxis(GIZMO_AXIS_RY), 0, 255, 0, 180); + if (bArcadeMode) { + //Use arcade class to determine left and right motor speeds + ArcadeTarget(GIZMO_AXIS_LX, GIZMO_AXIS_LY, targetL, targetR); + } else { + // Fetch the speed of each axis, then convert this to the range + // expected by the motors. + targetL = map(gizmo.getAxis(GIZMO_AXIS_LY), 0, 255, 0, 180); + targetR = map(gizmo.getAxis(GIZMO_AXIS_RY), 0, 255, 0, 180); + } + // Write the target speeds to the motor controllers. DriveL.write(targetL); DriveR.write(targetR); - // Sleep for 20ms, which means this loop will run at ~50Hz. This is - // because in this sample code all we do is get control inputs and - // map them to motors to drive around, so we don't need to go any - // faster. You should probably remove this in your code so that - // your main loop runs faster when polling sensors or animating - // lights. - delay(20); + // Update our claw position + Claw.update(); + + if (gizmo.getButton(GIZMO_BUTTON_RT)) { + //if right trigger is pressed, send arm motor forwards + Arm.write(180); + } else if (gizmo.getButton(GIZMO_BUTTON_RSHOULDER)) { + //if right shoulder is pressed, send ar motor backwards + Arm.write(0); + } else { + //if neither of thr right trigger buttons is pushed, let arm know to stop + Arm.write(90); + } + + // Sleep for 50ms, which means this loop will run at ~20Hz. + delay(50); } diff --git a/examples/BasicRobot/BasicRobotSources.zip b/examples/BasicRobot/BasicRobotSources.zip new file mode 100644 index 0000000..5dda1c3 Binary files /dev/null and b/examples/BasicRobot/BasicRobotSources.zip differ diff --git a/examples/BasicRobot/PositionalServo.h b/examples/BasicRobot/PositionalServo.h new file mode 100644 index 0000000..d3424eb --- /dev/null +++ b/examples/BasicRobot/PositionalServo.h @@ -0,0 +1,221 @@ +#ifndef PositionalServo_h +#define PositionalServo_h + +#include "Gizmo.h" + +//Positional Speed Values for the Servo. The higher selected, the faster the servo will go. +//These affect the number of degrees the servo jumps per interval plus how quickly it accelerates over time while its control is depressed +typedef enum { + PS_SPEED_NONE = -1, + PS_SPEED_SLOTH = 0, + PS_SPEED_SLOW, + PS_SPEED_MEDIUM, + PS_SPEED_FAST, + PS_SPEED_ULTRA +} PS_SPEED; + +/* Positionaal Servo Class. + * This class can be use to control a servo, maintining it at a set position or advancing it with the use of 2 buttons OR an axis. + * It can also add in accelearation the longer a control is held, helping give fine control or broad movements + */ +class PositionalServo { +public: + PositionalServo(); //Constructor + + /** + * init - This function does internal setup for the Positional Servo to be used with 2 buttons, and must be called during setup phase + * @param {int} motorId - The GIZMO_MOTOR_XXX port where the Servo is attached + * @param {int} btnA - used with btnB, the GIZMO_BUTTON_XXX value to decrement the servo position + * @param {int} btnB - used with btnB, the GIZMO_BUTTON_XXX value to advance the servo position + * @param {PS_SPEED} (OPTIONAL) - Speed servo should move, defaults to PS_SPEED_MEDIUM + * @param {int} (OPTIONAL) defpos - servo position, defaults to 90, right in the middle + * @param {bool} (OPTIONAL) useAcceleration - set to true iff the servo should speed up the longer a button or axis direction is held. Defaults to true + */ + void init(int motorId, int btnA, int btnB, PS_SPEED speed = PS_SPEED_MEDIUM, int defpos = 90, bool useAcceleration = true); + + + /** + * init - This function does internal setup for the Positional Servo to be used with an axis, and must be called during setup phase + * @param {int} motorId - The GIZMO_MOTOR_XXX port where the Servo is attached + * @param {int} Axis - by istelf, the GIZMO_AXIS_XXX to use to decrement or advance the Servo + * @param {PS_SPEED} (OPTIONAL) - Speed servo should move, defaults to PS_SPEED_MEDIUM + * @param {int} (OPTIONAL) defpos - servo position, defaults to 90, right in the middle + * @param {bool} (OPTIONAL) useAcceleration - set to true iff the servo should speed up the longer a button or axis direction is held. Defaults to true + */ + void init(int motorId, int Axis, PS_SPEED speed = PS_SPEED_MEDIUM, int defpos = 90, bool useAcceleration = true); + + /** + * setDefaultPosition - Set default position for the servo. Typically defaults to 90, right in the middle. + * This is only needed to override a previous default set in either the init call or to setDefaultPosition + * @param {int} defpos - set true to update the position of the servo to the enw default now + * @param {bool} (Optional) update - set to false to prevent the position from changing to the new defualt immediately + */ + void setDefaultPosition(int defpos, bool update=true); + + /** + * setSpeed - Set speed of the servo, defaults to PS_SPEED_MEDIUM. + * This is only needed to override a previous default set in either the init call or to setDefaultPosition + * @param {PS_SPEED} Speed - sets the speed the servo should move, from PS_SPEED_SLOTH to PS_SPEED_ULTRA, with PS_SPEED_SLOTH being the slowest + */ + void setSpeed(PS_SPEED Speed); + + /** + * setAcceleration - Set whether the servo should accelartion th3 longer a button or axis position is held, default to true. + * This is only needed to override a previous default set in either the init call or to setDefaultPosition + * @param {bool} useAcceleration - set to true if the servo should speed up the longer a button or axis direction is held + */ + void setAcceleration(bool useAcceleration); + + /** + * reset - Reset a servo back to its defualt position + */ + void reset(); + + /** + * update - Let a Servo process its position during the main loop + * @param {bool} (OPTIONAL) bReverse) - Set true to move the servo in the opposite direction + */ + void update(bool bReverse = false); + + private: + void init(int motorId, PS_SPEED speed, int defpos, bool useAcceleration); + int _initPos; + int _pos; + int _last; + int _step; + int _count; + int _btnUp; + int _btnDown; + int _Axis; + PS_SPEED _lastSpeed; + int _StepInt; + int _StepSize; + int _StepInc; + int _StepMax; + + Servo _motor; +}; + + +PositionalServo::PositionalServo() {} + +//Private Init function for inernal ffinal setup +void PositionalServo::init(int motorId, PS_SPEED speed, int defpos, bool useAcceleration) { + _last = _step = _count = 0; //reset all counters + _initPos = _pos = defpos; //Set initial and defual tpositions + _StepInt = useAcceleration ? 1 : -1; //Set initial step for accelerations + setSpeed(speed); //This call set final step interval, size and magnitude for accelration + _motor.attach(motorId); //Attach Motor + reset(); //Reset servo to default position to sync with these settings +} + +void PositionalServo::init(int motorId, int btnA, int btnB, PS_SPEED speed, int defpos, bool useAcceleration) { + //Init when using buttons + _btnUp = btnA; //Assign Bittons + _btnDown = btnB; + _Axis = -1; //Clear Axis + init(motorId, speed, defpos, useAcceleration); //Finish common init +} + +void PositionalServo::init(int motorId, int Axis, PS_SPEED speed, int defpos, bool useAcceleration) { + //Init when using an axis + _btnUp = _btnDown = -1; //CLear Buttons + _Axis = Axis; //Assign Axis + init(motorId, speed, defpos, useAcceleration); //Finish common init +} + +void PositionalServo::setDefaultPosition(int defPos, bool update) { + //Set a new default posiiton. + _initPos = defPos; + if (update) { reset(); } +} + + +void PositionalServo::setSpeed(PS_SPEED Speed) { + //Set servo speed. Servo speed is based on 3 factors: + // _StepInt - interval, how many loops have to go by before an update to acceleration is consiedered + // _StepSize - How many degrees to jump when changing positions, static when acceleration is not being used, otherwise growing + // _StepMax - Maximum step sie to grow to, when accelerating + _lastSpeed = Speed == PS_SPEED_NONE ? _lastSpeed : Speed; // If speed is none, this is just an update of the speed to reset Int, Size, and Max + switch(_lastSpeed) { + case PS_SPEED_SLOTH: //Go real slow + if (_StepInt>0) _StepInt = 5; + _StepSize = 2; + _StepMax = 30; + break; + + case PS_SPEED_SLOW: //Go kinda slow + if (_StepInt>0) _StepInt = 4; + _StepSize = 3; + _StepMax = 30; + break; + + case PS_SPEED_MEDIUM: //Go middle-ish + if (_StepInt>0) _StepInt = 3; + _StepSize = 6; + _StepMax = 40; + break; + + case PS_SPEED_FAST: //Go Kinda Fast + if (_StepInt>0) _StepInt = 2; + _StepSize = 10; + _StepMax = 50; + break; + + case PS_SPEED_ULTRA: //Go Fast + default: + if (_StepInt>0) _StepInt = 1; + _StepSize = 20; + _StepMax = 60; + break; + } + +} + +void PositionalServo::setAcceleration(bool useAcceleration) { + //Set whether to use acceleration or not. Setting Int below zero negates acceleration + _StepInt = useAcceleration ? 1 : -1; + setSpeed(PS_SPEED_NONE); +} + +void PositionalServo::reset() { + //Reset to out initial positions. Clear any counters + _pos = _initPos; + _last = _step = _count = 0; + _motor.write(_initPos); +} + +void PositionalServo::update(bool goReverse) { + //Cacluate and update our position, checking if controls are active + bool btnUp = false; + bool btnDown = false; + + if (_btnUp != -1 && _btnDown != -1) { //Using buttons controls, get buttin states from gizmo + btnUp = gizmo.getButton(_btnUp); + btnDown = gizmo.getButton(_btnDown); + } else if (_Axis != -1) { //Using axis controls, get axis state from gizmo, but just treat it as tertiary, its eitehr considered full tilt on way, the other, or in the middle + int val = map(gizmo.getAxis(_Axis), 0, 255, -1, 1); + btnUp = val < 0; + btnDown = val > 0; + } + if (btnUp) { //Up control is is use + if (_step <= 0) {_step = _StepSize; _count=0;} //If step has been zeroed or changed direction, reset it to beginning values and acceleration + _pos = MIN(180,_pos+_step); //Set our new position based on step, but make sure to not go over 180 + if (_StepInt > 0 && _count++ > _StepInt) { _count = 0; _step = MIN(_step+_StepSize,_StepMax); } //if it is time to acccelerate, up the step size, but don't go over max + } else if (btnDown) { //Down control is in use + if (_step >= 0) {_step = -1 * _StepSize; _count=0;} //If step has been zeroed or changed direction, reset it to beginning values and acceleration + _pos = MAX(0,_pos+_step); //Set our new position based on step, but make sure to not go below 0 + if (_StepInt > 0 && _count++ > _StepInt) { _count = 0; _step = MAX(_step-_StepSize,-1 * _StepMax); } //if it is time to acccelerate, up the step size, but don't go ov + } else { + //No control is use, zero out conters to reset any acceleration + _count = 0; + _step = 0; + } + + if (_last != _pos) { //Only write if a new position has been calculated + _motor.write(goReverse ? 180-_pos:_pos); //write it to the servo, reversing as needed + _last = _pos; //Store this new position for future checks + } +} + +#endif //PositionalServo_h diff --git a/examples/BasicRobot/StatefulButton.h b/examples/BasicRobot/StatefulButton.h new file mode 100644 index 0000000..562e62d --- /dev/null +++ b/examples/BasicRobot/StatefulButton.h @@ -0,0 +1,93 @@ +#ifndef StatefulButton_h +#define StatefulButton_h + +#include "Gizmo.h" + +/* Stateful Button Class. + * This class read values for an Input from the Gizmo, and can remember state for it to return + * transitions or current status. Sometimes referred to as a latching button. This class can be used on a button or a single side of an axis + */ +class StatefulButton { +public: + StatefulButton(); //Constrctor + + /** + * init - This function does internal setup for the Stateful Button, and must be called during setup phase + * @param {int} BtnId - The GIZMO_BUTTON_XXX id for this button to use + */ + void init(int BtnId); + + /** + * init - This function does internal setup for the Stateful Button, and must be called during setup phase + * @param {int} BtnId - The GIXMO_AXIS_XXX id for this button to use + * @param {SBAxisPos} AxisPos - When using GIXMO_AXIS_XXX as id, Set to false for the values below 90 on the Axis and true for those above + */ + void init(int AxisId, bool bLower); + + /** + * clicked - See if Stateful button has just been pressed. + * @returns {bool} - true iff the button has just been pressed and is now in the down position + */ + bool clicked(); + + /** + * released - See if Stateful button has just been released. + * @returns {bool} - true iff the button has just been released and is now in the up position + */ + bool released(); + + /** + * isDown - See if Stateful button is actively being pushed + * @returns {bool} - true iff the button has is currently down + */ + bool isDown(); + + private: + void update(); + int _id; + int _lastState; + int _state; + int _Axis; +}; + +StatefulButton::StatefulButton() {} + +void StatefulButton::init(int BtnId) { + //Init as a button + _id = BtnId; //Set button gizmo id + _Axis = 0; //zero axis use + _state = _lastState = false; //clear any state +} + +void StatefulButton::init(int AxisId, bool bLower) { + //init as an Axis + _id = AxisId; //Set button axis id + _Axis = bLower ? 1 : -1; //Mark if it is for the lower or upper portion of an axis + _state = _lastState = false; //clear any state +} + +bool StatefulButton::clicked() { + update(); //Update from gizmo + return _state && !_lastState; //We're only clicked when gizmo said button is down, when on the last check it wasn't +} + +bool StatefulButton::released() { + update(); //Update from gizmo + return !_state && _lastState; //We're only release when gizmo said button is up, when on the last check it wasn't +} + +bool StatefulButton::isDown() { + update(); //Update from gizmo + return _state; //Return current state +} + +void StatefulButton::update() { + _lastState = _state; //Remember our last state <-- this is the crux of this entire control + if (0 != _Axis) { //Using an axis control + int val = map(gizmo.getAxis(_id), 0, 255, -10, 10); //Read in the axis. We only worry if it is one side or the other + _state = _Axis == (val < -1 ? -1 : (val > 1 ? 1 : 0)); //Match it to our axis, -1 for below or 1 for above + } else { + _state = gizmo.getButton(_id); //using a button, just read its state + } +} +#endif //StatefulButton_h \ No newline at end of file diff --git a/examples/BasicRobot/Tasks.h b/examples/BasicRobot/Tasks.h new file mode 100644 index 0000000..b15be28 --- /dev/null +++ b/examples/BasicRobot/Tasks.h @@ -0,0 +1,120 @@ +#ifndef BasicTask_h +#define BasicTask_h + +typedef int (Task)(int); +typedef void (TaskSetup)(); + +/* Example usage for tasks Class + in main ino file, or your own other file, create function for Drive setup and loop + void DriveSetup() { + ... get ready to do cool stuff ... + } + + int DriveTask(int runStep) { + ... do cool stuff ... + } + + in main setup(), create your task with a name, address to a function to execute as its loop, and an optional setup function address + void setup() { + ... do other stuff + Tasks.Create("DRIVE", &DriveTask, &DriveSetup); + Tasks.Create("LEDS", &BlinkEm); + ... + Tasks.Create("ARM", &GoArmGo); + } + + in main loop(), ensure Tasks class runs all of its tasks, no delay needed - tasks takes care of that as long as everything runs in a task + void loop() { + Tasks.Update(); + } +*/ + + +// Managed subtask the TaskManager controls +class subTask { + public: + subTask() {_runStep = _nextTime =0; _loopFnc = NULL; _Next = NULL;}; + char* _name; //Subtask hman readable name + Task* _loopFnc; //Funciton to call when looping + int _runStep; //numebr of iteration loop has been called + int _nextTime; //time when it should call loop again + subTask* _Next; //Linked list pointer to next subtask in list +}; + +/* TaskManager Class. + * This class builds a manager to let independent tasks run on an Arduion IDE code base. Each task time slices, and returns back milliseconds until it wants to trun again + */ +class TaskManager { +public: + TaskManager(); //Constrctor + + /** + * Create - Create a new task, this is called during setup phase. + * @param {Task} loopFnc - Loop fucntion Task to call when running in loop mode. Of type int fnc(int). A Loop function should rturn an integer value indicating milliseconds until the next time it wants to run, and is passed in a count of the numbe rof iterations it has so far run + * @param {setupFnc} (OPTIONAL) setupFnc - void functon to call if setup is needed for the task, of type void fnc(). + */ + void Create(const char* name, Task* loopFnc, TaskSetup *setupFnc = NULL); + + /** + * Update - Update all tasks while running in loop + */ + void Update(); + + private: + subTask _subTasks; //Linked list of subtasks, initial one allocated in class +}; + +#include "Tasks.h" + +TaskManager::TaskManager() {} //Constructor + +void TaskManager::Create(const char *name, Task* loopFnc, TaskSetup* setupFnc) { + subTask* task = &_subTasks; //Gte head of our linked lists of tasks + + while (task->_loopFnc) { //Look until we find a subtask that doesn't yet has a mandatory loop function defined + if (!task->_Next) { //If we hit the end of the list, allocate a new one + task->_Next = new subTask(); + } + task = task->_Next; //Advance the list to next entry + } + + //This should be an empty subtask. Set it's name and loop function + task->_name = (char *)name; + task->_loopFnc = loopFnc; + + if (setupFnc) { //If it has a stup function, call it now + (*setupFnc)(); + } + +} + +void TaskManager::Update() { + //Update. Loop through our subtasks, and if any are scheduled to run, run them + int now = millis(); + int nextRun = now + 2000; //default to at least check every 2 seconds if we should run + + subTask* task = &_subTasks; //Start at the head of our subtasks list + + while (task) { + if (task->_loopFnc && task->_nextTime <= now) { //valid task says it should run becasue its next time is now or before now + task->_nextTime = millis() + (*(task->_loopFnc))(task->_runStep); //Execute its loop, getting back a value for when it should run again + task->_runStep = task->_runStep < 0x0FFFFFFF ? task->_runStep + 1 : 0; //Increment its runStep, ensuring not to overflow + } + //Serial.printf("TASK %s next in %d\r\n", task->_name, task->_nextTime - millis()); + nextRun = MIN(task->_nextTime, nextRun); //If its next time is hsorter than our expected next time, remeber it for setting overall delay + task = task->_Next; //Loop to the next subtask + } + + //Schedule our delay before allowing loop again + now = millis(); + int wait = nextRun > now ? nextRun - now : 1; + //Serial.printf("Delaying %d before next task eval\r\n", wait); + delay(wait); +} + +/* TaskManager + * Define tasks that can be time sliced and run independently. THey are created in Tasks, and then execute their loo funciton when their time is due + */ +TaskManager Tasks; + +#endif //BasicTask_h \ No newline at end of file