From 6727b4fdc95b1dddc163a7c7c5ee936b73fd26d3 Mon Sep 17 00:00:00 2001 From: David Baranyai Date: Wed, 6 Jun 2018 21:58:54 +0200 Subject: [PATCH] Working version --- .dep.inc | 5 + Makefile | 17 ++ joystick.cc | 83 ++++++++++ joystick.hh | 154 ++++++++++++++++++ main.cpp | 109 +++++++++++++ rpiPWM1.cpp | 431 +++++++++++++++++++++++++++++++++++++++++++++++++++ rpiPWM1.h | 177 +++++++++++++++++++++ rpiServo.cpp | 44 ++++++ rpiServo.h | 26 ++++ 9 files changed, 1046 insertions(+) create mode 100644 .dep.inc create mode 100644 Makefile create mode 100644 joystick.cc create mode 100644 joystick.hh create mode 100644 main.cpp create mode 100644 rpiPWM1.cpp create mode 100644 rpiPWM1.h create mode 100644 rpiServo.cpp create mode 100644 rpiServo.h diff --git a/.dep.inc b/.dep.inc new file mode 100644 index 0000000..38ba445 --- /dev/null +++ b/.dep.inc @@ -0,0 +1,5 @@ +# This code depends on make tool being used +DEPFILES=$(wildcard $(addsuffix .d, ${OBJECTFILES} ${TESTOBJECTFILES})) +ifneq (${DEPFILES},) +include ${DEPFILES} +endif diff --git a/Makefile b/Makefile new file mode 100644 index 0000000..5ce4691 --- /dev/null +++ b/Makefile @@ -0,0 +1,17 @@ +bt_car: main.o joystick.o rpiPWM1.o rpiServo.o + g++ -o bt_car joystick.o main.o rpiPWM1.o rpiServo.o -lwiringPi -lpthread + +main.o: main.cpp + g++ main.cpp -c -o main.o + +joystick.o: joystick.hh joystick.cc + g++ joystick.cc -c -o joystick.o + +rpiPWM1.o: rpiPWM1.h rpiPWM1.cpp + g++ rpiPWM1.cpp -c -o rpiPWM1.o + +rpiServo.o: rpiServo.h rpiServo.cpp + g++ rpiServo.cpp -c -o rpiServo.o + +clean: + rm *.o bt_car diff --git a/joystick.cc b/joystick.cc new file mode 100644 index 0000000..83463b0 --- /dev/null +++ b/joystick.cc @@ -0,0 +1,83 @@ +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Copyright Drew Noakes 2013-2016 + +#include "joystick.hh" + +#include +#include +#include +#include +#include +#include +#include "unistd.h" + +Joystick::Joystick() +{ + openPath("/dev/input/js0"); +} + +Joystick::Joystick(int joystickNumber) +{ + std::stringstream sstm; + sstm << "/dev/input/js" << joystickNumber; + openPath(sstm.str()); +} + +Joystick::Joystick(std::string devicePath) +{ + openPath(devicePath); +} + +Joystick::Joystick(std::string devicePath, bool blocking) +{ + openPath(devicePath, blocking); +} + +void Joystick::openPath(std::string devicePath, bool blocking) +{ + // Open the device using either blocking or non-blocking + _fd = open(devicePath.c_str(), blocking ? O_RDONLY : O_RDONLY | O_NONBLOCK); +} + +bool Joystick::sample(JoystickEvent* event) +{ + int bytes = read(_fd, event, sizeof(*event)); + + if (bytes == -1) + return false; + + // NOTE if this condition is not met, we're probably out of sync and this + // Joystick instance is likely unusable + return bytes == sizeof(*event); +} + +bool Joystick::isFound() +{ + return _fd >= 0; +} + +Joystick::~Joystick() +{ + close(_fd); +} + +std::ostream& operator<<(std::ostream& os, const JoystickEvent& e) +{ + os << "type=" << static_cast(e.type) + << " number=" << static_cast(e.number) + << " value=" << static_cast(e.value); + return os; +} + + diff --git a/joystick.hh b/joystick.hh new file mode 100644 index 0000000..d61d761 --- /dev/null +++ b/joystick.hh @@ -0,0 +1,154 @@ +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Copyright Drew Noakes 2013-2016 + +#ifndef __JOYSTICK_HH__ +#define __JOYSTICK_HH__ + +#include +#include + +#define JS_EVENT_BUTTON 0x01 // button pressed/released +#define JS_EVENT_AXIS 0x02 // joystick moved +#define JS_EVENT_INIT 0x80 // initial state of device + +/** + * Encapsulates all data relevant to a sampled joystick event. + */ +class JoystickEvent +{ +public: + /** Minimum value of axes range */ + static const short MIN_AXES_VALUE = -32768; + + /** Maximum value of axes range */ + static const short MAX_AXES_VALUE = 32767; + + /** + * The timestamp of the event, in milliseconds. + */ + unsigned int time; + + /** + * The value associated with this joystick event. + * For buttons this will be either 1 (down) or 0 (up). + * For axes, this will range between MIN_AXES_VALUE and MAX_AXES_VALUE. + */ + short value; + + /** + * The event type. + */ + unsigned char type; + + /** + * The axis/button number. + */ + unsigned char number; + + /** + * Returns true if this event is the result of a button press. + */ + bool isButton() + { + return (type & JS_EVENT_BUTTON) != 0; + } + + /** + * Returns true if this event is the result of an axis movement. + */ + bool isAxis() + { + return (type & JS_EVENT_AXIS) != 0; + } + + /** + * Returns true if this event is part of the initial state obtained when + * the joystick is first connected to. + */ + bool isInitialState() + { + return (type & JS_EVENT_INIT) != 0; + } + + /** + * The ostream inserter needs to be a friend so it can access the + * internal data structures. + */ + friend std::ostream& operator<<(std::ostream& os, const JoystickEvent& e); +}; + +/** + * Stream insertion function so you can do this: + * cout << event << endl; + */ +std::ostream& operator<<(std::ostream& os, const JoystickEvent& e); + +/** + * Represents a joystick device. Allows data to be sampled from it. + */ +class Joystick +{ +private: + void openPath(std::string devicePath, bool blocking=false); + + int _fd; + +public: + ~Joystick(); + + /** + * Initialises an instance for the first joystick: /dev/input/js0 + */ + Joystick(); + + /** + * Initialises an instance for the joystick with the specified, + * zero-indexed number. + */ + Joystick(int joystickNumber); + + /** + * Initialises an instance for the joystick device specified. + */ + Joystick(std::string devicePath); + + /** + * Joystick objects cannot be copied + */ + Joystick(Joystick const&) = delete; + + /** + * Joystick objects can be moved + */ + Joystick(Joystick &&) = default; + + /** + * Initialises an instance for the joystick device specified and provide + * the option of blocking I/O. + */ + Joystick(std::string devicePath, bool blocking); + + /** + * Returns true if the joystick was found and may be used, otherwise false. + */ + bool isFound(); + + /** + * Attempts to populate the provided JoystickEvent instance with data + * from the joystick. Returns true if data is available, otherwise false. + */ + bool sample(JoystickEvent* event); +}; + +#endif diff --git a/main.cpp b/main.cpp new file mode 100644 index 0000000..45ceaa6 --- /dev/null +++ b/main.cpp @@ -0,0 +1,109 @@ +/* + * To change this license header, choose License Headers in Project Properties. + * To change this template file, choose Tools | Templates + * and open the template in the editor. + */ + +/* + * File: main.cpp + * Author: pi + * + * Created on 2018. május 14., 10:10 + */ + +#include +#include "joystick.hh" +#include +#include +#include +//#include +//#include "rpiServo.h" + +#define PIN0 0 +#define PIN1 2 +#define PIN2 3 +#define PWM_RANGE 100 +#define SERVO_PWM_RANGE 50 +#define PWM_INIT_VALUE 0 + +using namespace std; + +/* + * + */ +int main(int argc, char** argv) +{ + wiringPiSetup(); + + char cmd[100]; + + //pinMode(0, PWM_OUTPUT); + int pwm0, pwm1, pwm2; + pwm0 = 0; + pwm1 = 0; + pwm2 = 0; + + //int pin0 = softPwmCreate(PIN0, PWM_INIT_VALUE, PWM_RANGE); + //int pin1 = softPwmCreate(PIN1, PWM_INIT_VALUE, PWM_RANGE); + //int pin2 = softPwmCreate(PIN2, PWM_INIT_VALUE, SERVO_PWM_RANGE); + + //softPwmWrite(0, 256); + + //pwmWrite(PIN0, 128); + + Joystick joy("/dev/input/js0"); + //rpiServo servo; + //servo.setAngle(30); + + while (true) + { + // Restrict rate + usleep(100); + + // Attempt to sample an event from the joystick + JoystickEvent event; + if (joy.sample(&event)) + { + if (event.isButton()) + { + printf("Button %u is %s\n", event.number, event.value == 0 ? "up" : "down"); + } + else if (event.isAxis()) + { + printf("Axis %u is at position %d\n", event.number, event.value); + if(event.number == 4) //forward + { + if(pwm1 == 0) + { + pwm0 = ((event.value+32768)/2620); + //softPwmWrite(PIN0, pwm0); + snprintf(cmd, 100, "echo 1=%d > /dev/servoblaster",-pwm0+150); + system(cmd); + } + } + if(event.number == 5) //backward + { + if(pwm0 == 0) + { + pwm1 = ((event.value+32768)/2620); + //softPwmWrite(PIN2, pwm1); + snprintf(cmd, 100, "echo 1=%d > /dev/servoblaster",pwm1+150); + system(cmd); + } + } + if(event.number == 0) //servo + { + pwm2 = (event.value/1092)+152; + //fprintf(fp, "2=%d",pwm2); + //fflush(fp); + snprintf(cmd, 100, "echo 2=%d > /dev/servoblaster",pwm2); + system(cmd); + std::cout << "PWM: " << pwm2 << std::endl; + } + + } + } + } + return 0; +} + diff --git a/rpiPWM1.cpp b/rpiPWM1.cpp new file mode 100644 index 0000000..3335be7 --- /dev/null +++ b/rpiPWM1.cpp @@ -0,0 +1,431 @@ +#include "rpiPWM1.h" + +//Need to do this to be able to access these constants +//Private constants +const int rpiPWM1::BCM2708_PERI_BASE; +const int rpiPWM1::PWM_BASE;/* PWM controller */ +const int rpiPWM1::CLOCK_BASE; /* Clock controller */ +const int rpiPWM1::GPIO_BASE; /* GPIO controller */ +const int rpiPWM1::PWM_CTL ; +const int rpiPWM1::PWM_RNG1; +const int rpiPWM1::PWM_DAT1; +const int rpiPWM1::PWMCLK_CNTL; +const int rpiPWM1::PWMCLK_DIV; +const int rpiPWM1::BLOCK_SIZE; +//Public constants +const int rpiPWM1::PWMMODE; +const int rpiPWM1::MSMODE; +const int rpiPWM1::ERRFREQ; +const int rpiPWM1::ERRCOUNT; +const int rpiPWM1::ERRDUTY; +const int rpiPWM1::ERRMODE; + + +/*********************************************************************** + * rpiPWM1::rpiPWM1() + * This is the Default constructor. First, it mmaps the registers in + * Physical memory responsible for configuring GPIO, PWM and the PWM clock. + * It then sets the frequency to 1KHz, PWM resolution to 256, duty + * cycle to 50% & pwm mode to 'PWMMODE' + * It then calls configPWM1Pin() to configure GPIO18 to ALT5 to allow it to + * output PWM1 waveforms. + * Finally configPWM1() is called to configure the PWM1 peripheral + ***********************************************************************/ +rpiPWM1::rpiPWM1() +{ + this->clk = mapRegAddr(CLOCK_BASE);// map PWM clock registers into memory + this->pwm = mapRegAddr(PWM_BASE); //map PWM registers into memory + this->gpio = mapRegAddr(GPIO_BASE);// map GPIO registers into memory + this->frequency = 1000.0; // set frequency + this->counts = 256; //set PWM resolution + this->dutyCycle = 50.0; //set duty cycle + this->mode = PWMMODE; // set pwm mode + configPWM1Pin(); //configure GPIO18 to ALT15 (PWM output) + configPWM1(); // configure PWM1 +} + +/*********************************************************************** + * rpiPWM1::rpiPWM1(double Hz, unsigned int cnts, double duty, unsigned int m) + * This is the overloaded constructor. First, it mmaps the registers in + * Physical memory responsible for configuring GPIO, PWM and the PWM clock. + * It then sets the frequency, PWM resolution, duty cycle & pwm mode as + * per the parameters provided. + * + * It then calls configPWM1Pin() to configure GPIO18 to ALT5 to allow it to + * output PWM1 waveforms. + * Finally configPWM1() is called to configure the PWM1 peripheral + * Parameters: - Hz (double) - Frequency + * - cnts (unsigned int) - PWM resolution (counts) + * - duty (double) - Duty Cycle as a percentage + * - m (int) - PWM mode (can be either 1 for PWMMODE (rpiPWM1::PWMMODE) + * or 2 for MSMODE (rpiPWM1::MSMODE) + ***********************************************************************/ +rpiPWM1::rpiPWM1(double Hz, unsigned int cnts, double duty, int m) +{ + this->clk = mapRegAddr(CLOCK_BASE); + this->gpio = mapRegAddr(GPIO_BASE); + this->pwm = mapRegAddr(PWM_BASE); + + if( (cnts < 0) || (cnts > UINT_MAX) ) { + printf("counts value must be between 0-%d\n",UINT_MAX); + exit(1); + } + + if ((Hz < 1e-5) || (Hz > 19200000.0f)){ + printf("frequency value must be between 0-19200000\n"); + exit(1); + } + + if( (duty < 1e-5) || (duty> 99.99999) ) { + printf("dutyCycle value must be between 0-99.99999\n"); + exit(1); + } + + if( (m != PWMMODE) && (m != MSMODE) ) { + printf("mode must be either PWMMODE(1) or MSMODE(2)\n"); + exit(1); + } + + this->frequency = Hz; + this->counts = cnts; + this->dutyCycle = duty; + this->mode = m; + configPWM1Pin(); + configPWM1(); +} + +/*********************************************************************** + * rpiPWM1::~rpiPWM1() + * Destructor - Puts all Peripheral registers in their original (reset state) + * and then unmaps the portions of memory containing to register addresses + * for the PWM clock, PWM and GPIO peripherals + ***********************************************************************/ +rpiPWM1::~rpiPWM1(){ + + + //lets put the PWM peripheral registers in their original state + *(pwm + PWM_CTL) = 0; + *(pwm + PWM_RNG1) = 0x20; + *(pwm + PWM_DAT1) = 0; + // unmap the memory block containing PWM registers + if(munmap((void*)pwm, BLOCK_SIZE) < 0){ + perror("munmap (pwm) failed"); + exit(1); + } + //lets put the PWM Clock peripheral registers in their original state + //kill PWM clock + *(clk + PWMCLK_CNTL) = 0x5A000000 | (1 << 5); + usleep(10); + + // wait until busy flag is set + while ( (*(clk + PWMCLK_CNTL)) & 0x00000080){} + + //reset divisor + *(clk + PWMCLK_DIV) = 0x5A000000; + usleep(10); + + // source=osc and enable clock + *(clk + PWMCLK_CNTL) = 0x5A000011; + + // unmap the memory block containing PWM Clock registers + if(munmap((void*)clk, BLOCK_SIZE) < 0){ + perror("munmap (clk) failed"); + exit(1); + } + + //lets put the GPIO peripheral registers in their original state + //first put it in input mode (default) + //taken from #define INP_GPIO(g) *(gpio+((g)/10)) &= ~(7<<(((g)%10)*3)) + *(gpio+1) &= ~(7 << 24); + //then munmap + if(munmap((void*)gpio, BLOCK_SIZE) < 0){ + perror("munmap (gpio) failed"); + exit(1); + } +} + +/*********************************************************************** + * unsigned int rpiPWM1::setFrequency(const double &hz) + * This function sets the PWM frequency and then reinitializes the PWM1 + * peripheral to update the frequency. The function performs a check to + * ensure that the PWM frequency is between 0 & 19.2MHz + * Parameters: hz (double) - Frequency in Hz + * Return Value: 0 if successful or rpiPWM1::ERRFREQ if frequency + * parameter is invalid + ***********************************************************************/ + unsigned int rpiPWM1::setFrequency(const double &hz){ + unsigned int retVal = 0; + if (hz < 1e-5 || hz > 19200000.0f){ // make sure that Frequency is valid + retVal = ERRFREQ; //if not return error code + } + else{ + this->frequency = hz; + configPWM1(); + } + return retVal; // return 0 for success..... +} + + +/*********************************************************************** + * unsigned int rpiPWM1::setCounts(const int &cnts) + * This function sets the PWM resolution and then reinitializes the PWM1 + * peripheral to update the PWM resolution (counts). The function performs a check to + * ensure that the PWM resolution is between 0 & UINT_MAX (its a 32-bit register) + * Parameters: cnts (unsigned int) - counts + * Return Value: 0 if successful or rpiPWM1::ERRCOUNT if count value is invalid + ***********************************************************************/ +unsigned int rpiPWM1::setCounts(const unsigned int &cnts){ + unsigned int retVal = 0; + + if( (cnts < 0) || (cnts > UINT_MAX) ) { + retVal = ERRCOUNT; + } + else{ + this->counts = cnts; + configPWM1(); + } + return retVal; +} + +/*********************************************************************** + * unsigned int rpiPWM1::setDutyCycle(const double &duty) + * This function sets the PWM DutyCycle while the PWM peripheral is running. + * The function performs a check to ensure that the PWM Duty Cycle is between + * 0 & 99.99999 % + * Parameters: duty (double) - Duty Cycle in % + * Return Value: 0 if successful or rpiPWM1::ERRDUTY if Duty cycle is invalid + ****************************************************************************/ +unsigned int rpiPWM1::setDutyCycle(const double &duty){ + unsigned int bitCount = 0; + unsigned int retVal = 0; + + if( (duty < 1e-5) || (duty > 99.99999) ) { + retVal = ERRDUTY; + } + else { + this->dutyCycle = duty; + bitCount = (int) ((this->dutyCycle/100.0) * this->counts); + *(pwm + PWM_DAT1) = bitCount; + } + + return retVal; +} + +/*********************************************************************** + * unsigned int rpiPWM1::setDutyCycleForce(const double &duty, unsigned int &m) + * This function firsts stops the PWM1 peripheral, sets the PWM DutyCycle + * and the PWM mode and then re-enables the PWM1 peripheral in the new mode + * The function performs a check to ensure that the PWM Duty Cycle is between + * 0 & 99.99999 % and that an appropriate mode is selected. + * + * Parameters: duty (double) - Duty Cycle in % + * m (int) - pwm mode (rpiPWM1::PWMMODE or rpiPWM1::MSMODE) + * Return Value: 0 if successful or rpiPWM1::ERRDUTY if Duty cycle is invalid + *******************************************************************************/ +unsigned int rpiPWM1::setDutyCycleForce(const double &duty, const int &m){ + int retVal = 0; + if( (m != PWMMODE) && (m != MSMODE) ) { + retVal = ERRMODE; + } + else if( (duty < 1e-5) || (duty > 99.99999) ) { + retVal = ERRDUTY; + } + else{ + this->mode = m; + this->dutyCycle = duty; + // disable PWM & start from a clean slate + *(pwm + PWM_CTL) = 0; + // needs some time until the PWM module gets disabled, without the delay the PWM module crashs + usleep(10); + // set the number of counts that constitute a period + *(pwm + PWM_RNG1) = this->counts; + //set duty cycle + *(pwm + PWM_DAT1) = (int) ((this->dutyCycle/100.0) * this->counts); + // start PWM1 in + if(this->mode == PWMMODE) //PWM mode + *(pwm + PWM_CTL) |= (1 << 0); + else // M/S Mode + *(pwm + PWM_CTL) |= ( (1 << 7) | (1 << 0) ); + } + return retVal; +} + +/*********************************************************************** + * unsigned int rpiPWM1::setDutyCycleCount(unsigned int &dutyCycleCnts ) + * This function sets the PWM DutyCycle as a function of PWM resolution, + * while the PWM peripheral is running. The function performs a check to + * ensure that the PWM Duty Cycle count value is between 0 and count + * Parameters: dutyCycleCnts (unsigned int) - Duty Cycle in counts + * Return Value:0 if successful or rpiPWM1::ERRDUTY if Duty cycle is invalid + ***********************************************************************/ +unsigned int rpiPWM1::setDutyCycleCount(const unsigned int &dutyCycleCnts ){ + unsigned int retVal = 0; + if( (dutyCycleCnts < 0) || ( dutyCycleCnts > this->counts )) { + retVal = ERRDUTY; + } + else{ + this->dutyCycle = ((dutyCycleCnts * 1.0)/ this->counts) * 100.0; + *(pwm + PWM_DAT1) = dutyCycleCnts; + } + return retVal; +} + +/*********************************************************************** + * unsigned int rpiPWM1::setMode(unsigned int &m) + * This function sets the PWM mode. The function performs a check to + * ensure that a valid PWM mode is requested. + * Parameters: m (int) - pwm mode (rpiPWM1::PWMMODE or rpiPWM1::MSMODE) + * Return Value: 0 if successful or rpiPWM1::ERRMODE if MODE is invalid + ********************************************************************************/ +unsigned int rpiPWM1::setMode(const int &m){ + unsigned int retVal = 0; + if( (m != PWMMODE) && (m != MSMODE) ) { + retVal = ERRMODE; + } + else{ + this->mode = m; + setDutyCycleForce(this->dutyCycle, this->mode); + } + + return retVal; + } + + +/*********************************************************************** + *These are a bunch of 'getter' functions that enable the user + * to access the PWM frequency, resolution, duty cycle and mode as well + * as the PWM clock divisor value. + *********************************************************************/ + double rpiPWM1::getFrequency() const{ return this->frequency;} + int rpiPWM1::getCounts() const { return this->counts;} + int rpiPWM1::getDivisor() const {return this->divisor;} + double rpiPWM1::getDutyCycle() const {return this->dutyCycle;} + int rpiPWM1::getMode() const{return this->mode;} + +/*********************************************************************** + * volatile unsigned *rpiPWM1::mapRegAddr(unsigned long baseAddr) + * This function maps a block (4KB) of physical memory into the memory of + * the calling process. It enables a user space process to access registers + * in physical memory directly without having to interact with in kernel side + * code i.e. device drivers + * Parameter - baseAddr (unsigned long) - this is the base address of a 4KB + * block of physical memory that will be mapped into the user + * space process memory. + * Return Value - mapped pointer in process memory + ***********************************************************************/ +volatile unsigned *rpiPWM1::mapRegAddr(unsigned long baseAddr){ + int mem_fd = 0; + void *regAddrMap = MAP_FAILED; + + /* open /dev/mem.....need to run program as root i.e. use sudo or su */ + if (!mem_fd) { + if ((mem_fd = open("/dev/mem", O_RDWR|O_SYNC) ) < 0) { + perror("can't open /dev/mem"); + exit (1); + } + } + + /* mmap IO */ + regAddrMap = mmap( + NULL, //Any adddress in our space will do + BLOCK_SIZE, //Map length + PROT_READ|PROT_WRITE|PROT_EXEC,// Enable reading & writting to mapped memory + MAP_SHARED|MAP_LOCKED, //Shared with other processes + mem_fd, //File to map + baseAddr //Offset to base address + ); + + if (regAddrMap == MAP_FAILED) { + perror("mmap error"); + close(mem_fd); + exit (1); + } + + if(close(mem_fd) < 0){ //No need to keep mem_fd open after mmap + //i.e. we can close /dev/mem + perror("couldn't close /dev/mem file descriptor"); + exit(1); + } + return (volatile unsigned *)regAddrMap; +} + + +/*********************************************************************** + * void rpiPWM1::configPWM1Pin() + * This function is responsible for putting GPIO18 in PWM mode by setting + * its alternate function to ALT5. It firsts make the GPIO an input + * (default state) and then switches to alternate function 5 (PWM mode) + ***********************************************************************/ +void rpiPWM1::configPWM1Pin() +{ + + /*GPIO 18 in ALT5 mode for PWM0 */ + // Let's first set pin 18 to input + //taken from #define INP_GPIO(g) *(gpio+((g)/10)) &= ~(7<<(((g)%10)*3)) + *(gpio+1) &= ~(7 << 24); + //then set it to ALT5 function PWM0 + //taken from #define SET_GPIO_ALT(g,a) *(gpio+(((g)/10))) |= (((a)<=3?(a)+4:(a)==4?3:2)<<(((g)%10)*3)) + *(gpio+1) |= (2<<24); + +} + + +/*********************************************************************** + * void rpiPWM1::configPWM1() + * This function configures the PWM1 peripheral. + * - It stops the PWM clock + * - Calculates an appropriate divisor value based on PWM Freq and resolution + * - Writes this divisor value to the PWM clock + * - Enables the PWM clock + * - Disables the PWM peripheral + * - Writes the PWM resolution and Duty Cycle to the appropriate registers + * - Enables the PWM peripheral in the requested mode + * *********************************************************************/ +void rpiPWM1::configPWM1(){ + + double period; + double countDuration; + + // stop clock and waiting for busy flag doesn't work, so kill clock + *(clk + PWMCLK_CNTL) = 0x5A000000 | (1 << 5); + usleep(10); + + // wait until busy flag is set + while ( (*(clk + PWMCLK_CNTL)) & 0x00000080){} + + //calculate divisor value for PWM1 clock...base frequency is 19.2MHz + period = 1.0/this->frequency; + countDuration = period/(counts*1.0f); + this->divisor = (int)(19200000.0f / (1.0/countDuration)); + + if( this->divisor < 0 || this->divisor > 4095 ) { + printf("divisor value must be between 0-4095\n"); + exit(-1); + } + + //set divisor + *(clk + PWMCLK_DIV) = 0x5A000000 | (this->divisor << 12); + + // source=osc and enable clock + *(clk + PWMCLK_CNTL) = 0x5A000011; + + // disable PWM & start from a clean slate + *(pwm + PWM_CTL) = 0; + + // needs some time until the PWM module gets disabled, without the delay the PWM module crashs + usleep(10); + + // set the number of counts that constitute a period with 0 for 20 milliseconds = 320 bits + *(pwm + PWM_RNG1) = this->counts; usleep(10); + + //set duty cycle + *(pwm + PWM_DAT1) = (int) ((this->dutyCycle/100.0) * this->counts); usleep(10); + + + // start PWM1 in + if(this->mode == PWMMODE) //PWM mode + *(pwm + PWM_CTL) |= (1 << 0); + else // M/S Mode + *(pwm + PWM_CTL) |= ( (1 << 7) | (1 << 0) ); +} + diff --git a/rpiPWM1.h b/rpiPWM1.h new file mode 100644 index 0000000..b0d05d2 --- /dev/null +++ b/rpiPWM1.h @@ -0,0 +1,177 @@ +#ifndef RPIPWM1_H + #define PRIPWM1_H +#include +#include +#include +#include +#include +#include +#include +#include +/*********************************************************************** + * Author: Hussam al-Hertani (Hertaville.com) + * Others are free to modify and use this code as they see fit so long as they + * give credit to the author. + * + * The author is not liable for any kind of damage caused by this software. + * + * Acknowledgements: This 'C++' class is based on 'C' code available from : + * - code from http://elinux.org/RPi_Low-level_peripherals + * - http://www.raspberrypi.org/phpBB3/viewtopic.php?t=8467&p=124620 for PWM initialization + * - frank's code...http://www.frank-buss.de/raspberrypi/pwm.c + * - Gertboard's C source code + * + * The rpiPWM1 class provides a direct memory mapped (register-based) + * interface to the PWM1 hardware on the Raspberry Pi's BCM2835 SOC. + * The BCM2835 SOC was two PWM subsystems, PWM1 & PWM2. This code will + * enable access to the PWM1 subsystem which outputs the PWM signal on + * GPIO18 (ALT5). + * + * The class enables setting the Frequency (Max 19.2MHz), PWM resolution(4095), + * DutyCycle and PWM Mode to be used. The Duty Cycle can be set as a + * percentage (setDutyCycle() or setDutyCycleForce()) or as a function + * of the PWM Resoultion (setDutyCycleCount()) + * + * Two PWM modes exist: + * - MSMODE - This is the traditional PWM Mode i.e. if PWM Resolution + * (counts) is 1024 and the dutyCycle (Pulse on time) is 512 (in counts or 50%) + * then the waveform would look like: + * |||||||||||||||||_________________ + * MSMODE is ideal for servos and other applications that + * require classical PWM waveforms + * - PWMMODE - Is a slightly modified version of the traditional PWM Mode + * described above. The duty cycle or ON time is still unchanged + * within the period but is distributed across the entire period instead + * on being concentrated in the first part of the period..i.e.if PWM Resolution + * (counts) is 1024 and the dutyCycle (Pulse on time) is 512 (in counts or 50%) + * then the waveform would look like: + * |_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_ + * This mode is ideal if you want to pass the signal through + * a low pass filter to obtain an analog signal equivalent to the . The even + * distibution of the ON time through the entire period + * significantly reduces ripple caused by using a simple RC + * low pass filter + * + * When setting the frequency via the constructor or 'setFrequency' method, one is strictly + * requesting a particular frequency. The code will do its best to get as close as possible to + * the requested frequency but it is likely to not create a PWM at the exact requested Frequency. + * As an example say that we want to create a PWM waveform with a resolution of 256 counts (8-bit) + * and a frequency of 8KHz....We get the divisor using the following algorithm + * + * Waveform period = 1 / 8KHz = 0.125ms + * Duration of a single count = period/256 = 0.125ms / 256 = 0.488us + * Frequency of a single count = 1 / Duration of a single count = 1 / 0.488us = 2.048MHz + * Divisor value = floor (PWM Clock Frequency / Frequency of a single count) = floor (19.2MHz / 2.048MHz) = floor(9.375) = 9 + * + * With a Divisor of 9 Actual Waveform Frequency = 1/((1/(19.2MHz/9))*256) = 8.333 KHz + * + * The actual Frequency will generally deviate further from the desired frequency as the count value (PWM resolution) + * increases i.e. As an example say that we want to create a PWM waveform with a resolution of 1024 counts (10-bit) + * and the same frequency as the above example: + * + * Waveform period = 1 / 8KHz = 0.125ms + * Duration of a single count = period/1024 = 0.125ms / 1024 = 122.070ns + * Frequency of a single count = 1 / Duration of a single count = 1 / 122.070ns = 8.192MHz + * Divisor value = floor (PWM Clock Frequency / Frequency of a single count) + * = floor (19.2MHz / 8.192MHz) = floor(2.34) = 2 + * + * With a Divisor of 2, Actual Waveform Frequency = 1/((1/(19.2MHz/2))*1024) = 9.375KHz + * + * DIVISOR MUST BE AT LEAST 2....SO PICK YOUR COUNT AND DESIRED FREQUENCY VALUES CAREFULLY!!!!! + * i.e MAXIMUM FREQUENCY FOR 10-BIT RESOLUTION (COUNT=1024) IS 9.375KHz + * & MAXIMUM FREQUENCY FOR 8-BIT RESOLUTION (COUNT=256) IS 37.5KHz + * + * WARNING: The RPI uses the PWM1 subsystem to produce audio. As such + * please refrain from playing audio on the RPI while this code + * is running. + * *********************************************************************/ +class rpiPWM1 { + +public: + rpiPWM1(); + // default constructor configures GPIO18 for PWM and Frequency 1000Hz, + // PWM resolution (counts) 256, Duty Cycle 50% and PWM mode is 'PWMMODE' + rpiPWM1(double Hz, unsigned int cnts, double duty, int m); + //overloaded constructor..allows user to set initial values for Frequency, + //PWM resolution, Duty Cycle and PWM mode. + ~rpiPWM1(); + // Destructor....safely releases all mapped memory and puts all used peripherals + // (PWM clock, PWM peripheral and GPIO peripheral in their initial states + unsigned int setFrequency(const double &hz); + // Sets Frequency and reinitializes PWM peripheral + unsigned int setCounts(const unsigned int &cnts); + // Sets PWM resolution (counts) and reinitializes PWM peripheral + unsigned int setDutyCycle(const double &duty); + // Sets Duty Cycle as a Percentage (Fast) + unsigned int setDutyCycleCount(const unsigned int &cnts ); + // Sets Duty Cycle as a count value (Fast) i.e. if counts is 1024 + // and 'duty' is set to 512, a 50% duty cycle is achieved + unsigned int setDutyCycleForce(const double &duty, const int &m); + // disables PWM peripheral first, + //Sets Duty Cycle as a Percentage and PWM mode... + // then enables PWM peripheral + unsigned int setMode(const int &m); + // sets PWM mode...calls 'setDutyCycleForce()' + + + double getFrequency() const; + // returns current Frequency of PWM waveform + double getDutyCycle() const; + // returns current DutyCycle (as a %) of PWM waveform + int getCounts() const; + // returns PWM resolution + int getDivisor() const; + //returns Divisor value used to set the period per count + //as a function of the default PWM clock Frequency of 19.2MHz + int getMode() const; + //returns (1) if current PWM mode is 'PWMMODE' or (2) if current PWM mode + //is 'MSMODE' + + //Public constants + static const int PWMMODE = 1; + static const int MSMODE = 2; + //Two PWM modes + static const int ERRFREQ = 1; + static const int ERRCOUNT = 2; + static const int ERRDUTY = 3; + static const int ERRMODE = 4; + //Error Codes + +private: + //Private constants + static const int BCM2708_PERI_BASE = 0x20000000; + static const int PWM_BASE = (BCM2708_PERI_BASE + 0x20C000); /* PWM controller */ + static const int CLOCK_BASE = (BCM2708_PERI_BASE + 0x101000); /* Clock controller */ + static const int GPIO_BASE = (BCM2708_PERI_BASE + 0x200000); /* GPIO controller */ + //Base register addresses + static const int PWM_CTL = 0; + static const int PWM_RNG1 = 4; + static const int PWM_DAT1 = 5; + static const int PWMCLK_CNTL= 40; + static const int PWMCLK_DIV = 41; + // Register addresses offsets divided by 4 (register addresses are word (32-bit) aligned + static const int BLOCK_SIZE = 4096; + // Block size.....every time mmap() is called a 4KB + //section of real physical memory is mapped into the memory of + //the process + + + volatile unsigned *mapRegAddr(unsigned long baseAddr); + // this function is used to map physical memory + void configPWM1Pin(); + //this function sets GPIO18 to the alternat function 5 (ALT5) + // to enable the pin to output the PWM waveforms generated by PWM1 + void configPWM1(); + //This function is responsible for the global configuration and initialixation + //of the the PWM1 peripheral + + double frequency; // PWM frequency + double dutyCycle; //PWM duty Cycle (%) + unsigned int counts; // PWM resolution + unsigned int divisor; // divisor value + int mode; // PWM mode + volatile unsigned *clk, *pwm, *gpio; // pointers to the memory mapped sections + //of our process memory + +}; +#endif diff --git a/rpiServo.cpp b/rpiServo.cpp new file mode 100644 index 0000000..f342a62 --- /dev/null +++ b/rpiServo.cpp @@ -0,0 +1,44 @@ +#include "rpiServo.h" + +const int rpiServo::ERRDEG;// Error code + +/********************************************************************************* + * rpiServo constructor - Calls rpiPWM1's overloaded constructor + * which basically creates a 50Hz PWM waveform, wih a resolution count of 3600, + * a duty cycle of 7.5% (1.5ms ON pulse every 20ms to center servo shaft at + * 90 degree position) and traditional PWM mode (rpiPWM1::MSMODE) + * + * By setting count resolution to 3600 counts, one 20ms period is equivalent to + * to 3600 counts, 2ms ON time (180 degree position) is equivalent to 360 counts + * and 1ms ON time (0 degree position) is equivalent to 180 counts. This gives us + * 180 counts between the 0 degree position and the 180 degree position hence we + * get 1 degree rotation resolution + * + *********************************************************************************/ + +rpiServo::rpiServo():rpiPWM1(50.0,3600,7.5,rpiPWM1::MSMODE){ + // 20ms = 3600 counts (Period) + // 2ms = 360 counts (10% duty cycle) => angle 180 + // 1ms = 180 counts (5% duty cycle) => angle 0 + // 1.5ms = 180+90 = 270 (7.5% duty cycle ) => angle = 90 //servo centered +} + + +/************************************************************************ + *setAngle() - function sets the angle of the servo's shaft + *Parameters - degrees - new shaft angle. Can be any value between 0 and 180 only + * + *return Value - If parameter 'degrees' was between 0 and 180 (inclusive) then + return value is 0 else its spiServo::ERRDEG + ***********************************************************************/ + +unsigned int rpiServo::setAngle(unsigned int degrees){ + unsigned int retVal = 0; + + if((degrees < 0) || (degrees > 180 )) + retVal = ERRDEG; + else + this->setDutyCycleCount(180 + degrees); // call the necessary rpiPWM1 method + + return retVal; +} diff --git a/rpiServo.h b/rpiServo.h new file mode 100644 index 0000000..0658190 --- /dev/null +++ b/rpiServo.h @@ -0,0 +1,26 @@ +#ifndef RPI_SERVO_H +#define RPI_SERVO_H + +#include "rpiPWM1.h" + +/****************************************************************** + * rpiServo - This tiny C++ class is able to generate waveforms necessary to + * control servos. This class is derived from the rpiPWM1 class. + * It consists of a constructor that creates a 50Hz PWM waveform with + * a 1.5ms ON time pulse...causing the servo to center itself to the + * 90 degree position. + * + * The class also consists on a setAngle method that sets the angle of + * rotation to anything between 0 & 180 degrees + * ****************************************************************/ + +class rpiServo : public rpiPWM1 +{ + +public: + rpiServo(); + unsigned int setAngle(unsigned int degrees); + static const int ERRDEG=1; +}; + +#endif