<commit_before>#include "Controller.h"

#include "boost/thread.hpp"
#include "boost/bind.hpp"
#include "boost/progress.hpp"


#include "comm/FalconCommLibFTDI.h"
#include "grip/FalconGripFourButton.h"
#include "firmware/FalconFirmwareNovintSDK.h"

#include "kinematic/stamper/InverseKinematic.h"
#include "kinematic/stamper/JacobianMatrix.h"

#include <iostream>

namespace controller
{

	Controller::Controller()
	{
		falconKinematic = new libnifalcon::FalconKinematicStamper(false);
		std::cout << "Falcon initialization" << std::endl;
		//Don't set a kinematics model, since we're handing that ourselves
		falconModel = new libnifalcon::FalconDeviceBoostThread();
		int8_t i;
		falconModel->setFalconComm<libnifalcon::FalconCommLibFTDI>();
		falconModel->setFalconFirmware<libnifalcon::FalconFirmwareNovintSDK>();
		falconModel->setFalconGrip<libnifalcon::FalconGripFourButton>();

		if(!falconModel->open(0))
		{
			std::cout << "Can't open!" << std::endl;
		}
		
		falconModel->getFalconFirmware()->setHomingMode(true);
		int8_t count;
		if(!falconModel->isFirmwareLoaded())
		{
			std::cout << "Loading firmware..." << std::endl;
			std::cout << falconModel->setFirmwareFile("test_firmware.bin") << std::endl;		
			for(int i = 0; i < 10; ++i)
			{			
				if(falconModel->loadFirmware(10)) break;
			}
		}
		std::cout << "Kinematics initialization" << std::endl;
		if(falconModel->isFirmwareLoaded())
		{
			falconKinematic->initialize();
			std::cout << "Initialization finished" << std::endl;
			//boost::thread thread(boost::bind( &libnifalcon::FalconDevice::runThreadLoop, falconModel));
			falconModel->startThread();
		}
	}

	Controller::~Controller()
	{
		delete toolbox;
		delete viewer;
		delete falconModel;
		delete falconView;
	}

	void Controller::display()
	{
		if (viewer)
			viewer->display();
	}

	void Controller::reshape(int w, int h)
	{
		if (viewer)
			viewer->reshape(w, h);
	}

	void Controller::mouseButton(int button, int state, int x, int y)
	{
		if (viewer)
			viewer->mouseButton(button, state, x, y);
	}

	void Controller::mouseMove(int x, int y)
	{
		if (viewer)
			viewer->mouseMove(x, y);
	}

	void Controller::keyboard(unsigned char key, int x, int y)
	{
		if (viewer)
			viewer->keyboard(key, x, y);
	}

	void Controller::idle()
	{

		if(!falconModel->getFalconFirmware()->isHomed())
		{
			falconModel->getFalconFirmware()->setLEDStatus(libnifalcon::FalconFirmware::RED_LED);
			return;
		}
		else
		{
			falconModel->getFalconFirmware()->setLEDStatus(libnifalcon::FalconFirmware::GREEN_LED);
		}
		int16_t* encoders = new int16_t[3];
		encoders = falconModel->getFalconFirmware()->getEncoderValues();
		angle1 = falconKinematic->getTheta(encoders[0]);
		angle2 = falconKinematic->getTheta(encoders[1]);
		angle3 = falconKinematic->getTheta(encoders[2]);
	
		if (toolbox)
			toolbox->sync();

		gmtl::Vec3f angle_vec(gmtl::Math::deg2Rad(angle1), gmtl::Math::deg2Rad(angle2), gmtl::Math::deg2Rad(angle3));
	gmtl::Point3f position = falconKinematic->getDirectKinematic()->calculate(angle_vec);
	
		falconView->getEffector()->setPosition(position);
	
		libnifalcon::StamperKinematicImpl::Angle angle = libnifalcon::StamperKinematicImpl::InverseKinematic::calculate(position);
	
		falconView->getArm(view::Falcon::ARM_1)->setAngle(angle.theta1[view::Falcon::ARM_1]);
		falconView->getArm(view::Falcon::ARM_2)->setAngle(angle.theta1[view::Falcon::ARM_2]);
		falconView->getArm(view::Falcon::ARM_3)->setAngle(angle.theta1[view::Falcon::ARM_3]);
	
		falconView->getParallel(view::Falcon::ARM_1)->update(angle.theta1[view::Falcon::ARM_1],
															 angle.theta2[view::Falcon::ARM_1],
															 angle.theta3[view::Falcon::ARM_1]);<commit_msg>Changed controller to use binary firmware headers for firmware loading Fixed position coordinate scaling<commit_after>#include "Controller.h"

#include "boost/thread.hpp"
#include "boost/bind.hpp"
#include "boost/progress.hpp"


#include "falcon/comm/FalconCommLibFTDI.h"
#include "falcon/grip/FalconGripFourButton.h"
#include "falcon/firmware/FalconFirmwareNovintSDK.h"
#include "falcon/util/FalconFirmwareBinaryNvent.h"

#include "falcon/kinematic/stamper/InverseKinematic.h"
#include "falcon/kinematic/stamper/JacobianMatrix.h"

#include <iostream>

namespace controller
{

	Controller::Controller()
	{
		falconKinematic = new libnifalcon::FalconKinematicStamper(false);
		std::cout << "Falcon initialization" << std::endl;
		//Don't set a kinematics model, since we're handing that ourselves
		falconModel = new libnifalcon::FalconDeviceBoostThread();
		int8_t i;
		falconModel->setFalconComm<libnifalcon::FalconCommLibFTDI>();
		falconModel->setFalconFirmware<libnifalcon::FalconFirmwareNovintSDK>();
		falconModel->setFalconGrip<libnifalcon::FalconGripFourButton>();

		if(!falconModel->open(0))
		{
			std::cout << "Can't open!" << std::endl;
		}
		
		falconModel->getFalconFirmware()->setHomingMode(true);
		int8_t count;
		if(!falconModel->isFirmwareLoaded())
		{
			std::cout << "Loading firmware..." << std::endl;
			for(int i = 0; i < 10; ++i)
			{			
				if(!falconModel->getFalconFirmware()->loadFirmware(true, NOVINT_FALCON_NVENT_FIRMWARE_SIZE, const_cast<uint8_t*>(NOVINT_FALCON_NVENT_FIRMWARE)))
				{
					std::cout << "Could not load firmware" << std::endl;
				}
				else
				{
					break;
				}
			}
		}
		std::cout << "Kinematics initialization" << std::endl;
		if(falconModel->isFirmwareLoaded())
		{
			falconKinematic->initialize();
			std::cout << "Initialization finished" << std::endl;
			//boost::thread thread(boost::bind( &libnifalcon::FalconDevice::runThreadLoop, falconModel));
			falconModel->startThread();
		}
	}

	Controller::~Controller()
	{
		delete toolbox;
		delete viewer;
		delete falconModel;
		delete falconView;
	}

	void Controller::display()
	{
		if (viewer)
			viewer->display();
	}

	void Controller::reshape(int w, int h)
	{
		if (viewer)
			viewer->reshape(w, h);
	}

	void Controller::mouseButton(int button, int state, int x, int y)
	{
		if (viewer)
			viewer->mouseButton(button, state, x, y);
	}

	void Controller::mouseMove(int x, int y)
	{
		if (viewer)
			viewer->mouseMove(x, y);
	}

	void Controller::keyboard(unsigned char key, int x, int y)
	{
		if (viewer)
			viewer->keyboard(key, x, y);
	}

	void Controller::idle()
	{

		if(!falconModel->getFalconFirmware()->isHomed())
		{
			falconModel->getFalconFirmware()->setLEDStatus(libnifalcon::FalconFirmware::RED_LED);
			return;
		}
		else
		{
			falconModel->getFalconFirmware()->setLEDStatus(libnifalcon::FalconFirmware::GREEN_LED);
		}
		int16_t* encoders = new int16_t[3];
		encoders = falconModel->getFalconFirmware()->getEncoderValues();
		angle1 = falconKinematic->getTheta(encoders[0]);
		angle2 = falconKinematic->getTheta(encoders[1]);
		angle3 = falconKinematic->getTheta(encoders[2]);
	
		if (toolbox)
			toolbox->sync();

		gmtl::Vec3f angle_vec(gmtl::Math::deg2Rad(angle1), gmtl::Math::deg2Rad(angle2), gmtl::Math::deg2Rad(angle3));
		gmtl::Point3f position = falconKinematic->getDirectKinematic()->calculate(angle_vec);
		gmtl::Point3f position_new = position;
		position_new *= 1000.0;
		falconView->getEffector()->setPosition(position_new);
	
		libnifalcon::StamperKinematicImpl::Angle angle = libnifalcon::StamperKinematicImpl::InverseKinematic::calculate(position);
	
		falconView->getArm(view::Falcon::ARM_1)->setAngle(angle.theta1[view::Falcon::ARM_1]);
		falconView->getArm(view::Falcon::ARM_2)->setAngle(angle.theta1[view::Falcon::ARM_2]);
		falconView->getArm(view::Falcon::ARM_3)->setAngle(angle.theta1[view::Falcon::ARM_3]);
	
		falconView->getParallel(view::Falcon::ARM_1)->update(angle.theta1[view::Falcon::ARM_1],
															 angle.theta2[view::Falcon::ARM_1],
															 angle.theta3[view::Falcon::ARM_1]);
{'fraction_non_alphanumeric': 0.15861534487212606, 'fraction_numerical': 0.011754068716094032, 'mean_word_length': 5.534177215189874, 'pattern_counts': {'":': 0, '<': 62, '<?xml version=': 0, '>': 94, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '6587209', 'n_tokens_mistral': 3370, 'n_tokens_neox': 2929, 'n_words': 397}