olcPixelGameEngine/Videos/OneLoneCoder_PGE_RobotArm1.cpp

/*
	Programming a robotic arm
	"I told you, put down the screwdriver..." - javidx9

	License (OLC-3)
	~~~~~~~~~~~~~~~

	Copyright 2018-2019 OneLoneCoder.com

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

	1. Redistributions or derivations of source code must retain the above
	copyright notice, this list of conditions and the following disclaimer.

	2. Redistributions or derivative works in binary form must reproduce
	the above copyright notice. This list of conditions and the following
	disclaimer must be reproduced in the documentation and/or other
	materials provided with the distribution.

	3. Neither the name of 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 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 COPYRIGHT
	HOLDER 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.

	Instructions:
	~~~~~~~~~~~~~
	Without a robot arm and an mbed there is not much you can do!
	Also requires a 3rd Party PGEX UI by ZleapingBear:
	https://youtu.be/bfiSjC__MCI
	

	Relevant Video: https://youtu.be/ekdQ-aAB36Y

	Links
	~~~~~
	YouTube:	https://www.youtube.com/javidx9
				https://www.youtube.com/javidx9extra
	Discord:	https://discord.gg/WhwHUMV
	Twitter:	https://www.twitter.com/javidx9
	Twitch:		https://www.twitch.tv/javidx9
	GitHub:		https://www.github.com/onelonecoder
	Patreon:	https://www.patreon.com/javidx9
	Homepage:	https://www.onelonecoder.com

	Author
	~~~~~~
	David Barr, aka javidx9, <EFBFBD>OneLoneCoder 2019
*/

#define OLC_PGE_APPLICATION
#include "olcPixelGameEngine.h"
#include "olcPGEX_UI.h"

class RobotArm1 : public olc::PixelGameEngine
{
public:
	RobotArm1()
	{
		sAppName = "Robot Arm 1";
	}

	olc::UI_CONTAINER gui;
	float fJointAngle[6];
	float fAccumulatedTime = 0.0f;
	HANDLE hCom = nullptr;


public:
	bool OnUserCreate() override
	{
		gui.addSlider(10, 20, 180);
		gui.addSlider(10, 60, 180);
		gui.addSlider(10, 100, 180);
		gui.addSlider(10, 140, 180);
		gui.addSlider(10, 180, 180);
		gui.addSlider(10, 220, 180);
		gui.setValue(0, 50);
		gui.setValue(1, 50);
		gui.setValue(2, 50);
		gui.setValue(3, 50);
		gui.setValue(4, 50);
		gui.setValue(5, 50);

		// Open COM Port
		hCom = CreateFile("COM3", GENERIC_READ | GENERIC_WRITE, 0, nullptr, OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL, nullptr);
		if (hCom == nullptr) return false;

		// Configure Protocol: 9600bps, 8N1
		DCB dcb = { 0 };
		GetCommState(hCom, &dcb);
		dcb.BaudRate = CBR_9600;
		dcb.ByteSize = 8;
		dcb.StopBits = ONESTOPBIT;
		dcb.Parity = NOPARITY;
		SetCommState(hCom, &dcb);		
		return true;
	}

	bool OnUserDestroy() override
	{
		if (hCom != nullptr) CloseHandle(hCom);

		return true;
	}

	bool OnUserUpdate(float fElapsedTime) override
	{
		gui.Update(fElapsedTime);
		Clear(olc::GREEN);
		gui.drawUIObjects();

		for (int i = 0; i < 6; i++)
			fJointAngle[i] = (gui.getSliderFloat(i) / 100.0f) * 180.0f - 90.0f;

		unsigned char command[12];
		for (int i = 0; i < 6; i++)
		{
			command[i * 2 + 0] = i;
			command[i * 2 + 1] = (int)(128 + fJointAngle[i]);
		}

		fAccumulatedTime += fElapsedTime;
		if (fAccumulatedTime > 0.05f)
		{
			fAccumulatedTime -= 0.05f;
			DWORD bw = 0;
			WriteFile(hCom, command, 12, &bw, 0);
		}

		return true;
	}
};

int main()
{
	RobotArm1 demo;
	if (demo.Construct(400, 400, 2, 2))
		demo.Start();
	return 0;
}

// Below here is the source code compiled on MBED LPC1768, using the BufferedSerial Library

/*

#include "mbed.h"
#include "BufferedSerial.h"

PwmOut pin26(p26);
PwmOut pin25(p25);
PwmOut pin24(p24);
PwmOut pin23(p23);
PwmOut pin22(p22);
PwmOut pin21(p21);

BufferedSerial uart(p9, p10);


class Joint
{
private:
    static const float fDutyMin = 0.03f; // -90
    static const float fDutyMax = 0.11f; // +90
    static const float fDutyRange = fDutyMax - fDutyMin;
    
    float fTarget;
    float fPosition;
    float fJointMax;
    float fJointMin;
    
public: 
    Joint(float fMin = -90.0f, float fMax = 90.0f, float fDefaultPos = 0.0f)
    {
        fJointMin = fMin;
        fJointMax = fMax;
        fPosition = 0.0f;
        SetTarget(fDefaultPos);
    }

    void SetTarget(float fAngle)
    {
        fTarget = fAngle;
        if(fTarget < fJointMin) fTarget = fJointMin;
        if(fTarget > fJointMax) fTarget = fJointMax;
    }
    
    void UpdatePosition()
    {
        fPosition = fTarget;
    }
    
    float GetTarget()
    {
        return fTarget;
    }
    
    float GetDutyCycle()
    {
        float fDutyCycle = fPosition / (fJointMax - fJointMin);
        fDutyCycle = (fDutyCycle * fDutyRange) + fDutyMin + (fDutyRange * 0.5f);
        return fDutyCycle;
    }
};


int main() 
{
    // Servos (MG996R) operate on 20ms period, so set
    // PWM period for each pin
    pin26.period(0.02f);
    pin25.period(0.02f);
    pin24.period(0.02f);
    pin23.period(0.02f);
    pin22.period(0.02f);
    pin21.period(0.02f);
    
    Joint joint[6];
    
    joint[0].SetTarget(0.0f);
    joint[1].SetTarget(0.0f);
    joint[2].SetTarget(0.0f);
    joint[3].SetTarget(-25.0f);
    joint[4].SetTarget(-20.0f);
    joint[5].SetTarget(-15.0f);
    
    int nTargetJoint = 0;
    
    while(1)
    {
        // Read from UART
        if(uart.readable())
        {
            unsigned char c = (unsigned char)uart.getc();
             
            if(c < 10)
               nTargetJoint = c;
            else
               joint[nTargetJoint].SetTarget((float)c - 128);   
               
        }
        
        
        // Write Duty Cycles
        // Update each joints position
        for(int i=0; i<6; i++)
            joint[i].UpdatePosition();
            
        // Set PWM values for each joint
        pin26.write(joint[0].GetDutyCycle());   
        pin25.write(joint[1].GetDutyCycle());   
        pin24.write(joint[2].GetDutyCycle());   
        pin23.write(joint[3].GetDutyCycle());   
        pin22.write(joint[4].GetDutyCycle());           
        pin21.write(joint[5].GetDutyCycle());   
        
    }   
}

*/