commit 6727b4fdc95b1dddc163a7c7c5ee936b73fd26d3 Author: David Baranyai Date: Wed Jun 6 21:58:54 2018 +0200 Working version 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