void aimAtTargetFORWARD(int error) { printf("Attempting to aim at target! Error: %d\n", error); setRun(RIGHT_WHEEL_INDEX, 1); setRun(LEFT_WHEEL_INDEX, 1); setDuty(LEFT_WHEEL_INDEX, NEUTRAL_DUTY - (error * 50) + (ADJUST_RATE / 2)); setDuty(RIGHT_WHEEL_INDEX, NEUTRAL_DUTY + (error * 50) - (ADJUST_RATE / 2)); }
static void motorTask(void *params) { Motor *motor = (Motor*)params; portTickType xLastWakeTime; s32 pos, targetPos; u8 stopped; s32 currentDuty = 0; u16 maxDutyChange = MAX_DUTY * motor->pollingPeriod / ACCEL_TIME; xLastWakeTime = xTaskGetTickCount(); for (;;) { xSemaphoreTake(motor->lock, portMAX_DELAY); pos = getPosition(motor->currentPosition); targetPos = motor->targetPosition; stopped = motor->stopped; xSemaphoreGive(motor->lock); if (stopped) { // immediately stop the motor if (currentDuty >= maxDutyChange) currentDuty -= maxDutyChange; else if (currentDuty <= -maxDutyChange) currentDuty += maxDutyChange; else currentDuty = 0; setDuty(motor, currentDuty); } else if (targetPos > pos) { // We have to increase the position to reach the target setDirection(motor->currentPosition, Up); currentDuty = min(maxDutyAtDistance(targetPos - pos), currentDuty + maxDutyChange); setDuty(motor, currentDuty); } else if (targetPos < pos) { // We have to decrease the position to reach the target setDirection(motor->currentPosition, Down); currentDuty = -min(maxDutyAtDistance(pos - targetPos), -currentDuty + maxDutyChange); setDuty(motor, currentDuty); } else { // We have reached the target setDirection(motor->currentPosition, Unknown); currentDuty = 0; setDuty(motor, currentDuty); } vTaskDelayUntil(&xLastWakeTime, motor->pollingPeriod); } }
//--------------------------------------------------- TremoloDSP::TremoloDSP( XMLParser& parser, XMLNode* tremoloNode ) : DSP( MONKY_DSP_TYPE_TREMOLO ) { const float NO_VALUE_SPECIFIED = -1000000000.0f; parser.validateXMLAttributes( tremoloNode, "", "type,frequency,depth,shape,timeSkewing,duty,flatness,phase,spread" ); float frequency = parser.getXMLAttributeAsFloat( tremoloNode, "frequency", NO_VALUE_SPECIFIED ); float depth = parser.getXMLAttributeAsFloat( tremoloNode, "depth", NO_VALUE_SPECIFIED ); float shape = parser.getXMLAttributeAsFloat( tremoloNode, "shape", NO_VALUE_SPECIFIED ); float timeSkewing = parser.getXMLAttributeAsFloat( tremoloNode, "timeSkewing", NO_VALUE_SPECIFIED ); float duty = parser.getXMLAttributeAsFloat( tremoloNode, "duty", NO_VALUE_SPECIFIED ); float flatness = parser.getXMLAttributeAsFloat( tremoloNode, "flatness", NO_VALUE_SPECIFIED ); float phase = parser.getXMLAttributeAsFloat( tremoloNode, "phase", NO_VALUE_SPECIFIED ); float spread = parser.getXMLAttributeAsFloat( tremoloNode, "spread", NO_VALUE_SPECIFIED ); if( frequency != NO_VALUE_SPECIFIED ) setFrequency( frequency ); if( depth != NO_VALUE_SPECIFIED ) setDepth( depth ); if( shape != NO_VALUE_SPECIFIED ) setShape( shape ); if( timeSkewing != NO_VALUE_SPECIFIED ) setTimeSkewing( timeSkewing ); if( duty != NO_VALUE_SPECIFIED ) setDuty( duty ); if( flatness != NO_VALUE_SPECIFIED ) setFlatness( flatness ); if( phase != NO_VALUE_SPECIFIED ) setPhase( phase ); if( spread != NO_VALUE_SPECIFIED ) setSpread( spread ); }
//COMMANDE PWM ///////////////////////////////////////////////////////////// void Servo::commande(const std_msgs::Float64::ConstPtr& msg) { float input = msg->data; float commandeNs = zero + input * k; if(type== "direction" || "moteur" || "selecteur") { if(commandeNs < dutyMax && commandeNs > dutyMin) { duty = (int)commandeNs; setDuty(duty); } else { std::cout << "(Servo::commande)["<< type <<"] valeur limites depassees" << std::endl; } } else { std::cout << "(Servo::commande) : erreur de type !" << std::endl; } }
// CREATE Timer T1 PWM to drive inverter for regulated Geiger tube voltage void Inverter::initPWM() { TCCR1A = 0; // disable all PWM on Timer1 whilst we set it up DDRB |= _BV(PB5); // Set PB5 as output (PB5 is OC1A) ICR1 = F_CPU / freq; // set the frequency FCPU/(ICR1 * PRESCALLING) Hz . Prescalling set to 1X setDuty(duty); TCCR1B = (1 << WGM13) | (1<<WGM12) | (1 << CS10); //Fast PWM mode via ICR1, with no prescaling (CS11 = 8x, CS10 = 1x) TCCR1A |= (1 << COM1A1) | (1<< CS11); // set none-inverting mode and start PWM }
void DutyUnit::loadState(const SaveState::SPU::Duty &dstate, const unsigned nr1, const unsigned nr4, const unsigned long cc) { nextPosUpdate = std::max(dstate.nextPosUpdate, cc); pos = dstate.pos & 7; setDuty(nr1); period = toPeriod((nr4 << 8 & 0x700) | dstate.nr3); enableEvents = true; setCounter(); }
PwmSysfsDriverNode::PwmSysfsDriverNode(ros::NodeHandle const& nh, ros::NodeHandle const& nh_rel): nh_(nh), nh_rel_(nh_rel) { // get reqired path to sysfs pwm directory parameter std::string pwm_sysfs_dir; if (!nh_rel_.getParam("pwm_sysfs_dir", pwm_sysfs_dir)) { ROS_FATAL("Path to sysfs pwm directory required."); ros::shutdown(); return; } // attemp to create driver try { driver_.reset(new PwmSysfsDriver(pwm_sysfs_dir)); } catch (PwmSysfsDriverException e) { driver_.reset(); ROS_FATAL("Failed to initialize driver: %s", e.what()); ros::shutdown(); return; } // set initial pwm values bool enable; if (nh_rel_.getParam("initial_enable", enable)) setEnable(enable); bool invert_polarity; if (nh_rel_.getParam("initial_invert_polarity", invert_polarity)) setInvertPolarity(invert_polarity); double period; if (nh_rel_.getParam("initial_period", period)) setPeriod(period); double duty; if (nh_rel_.getParam("initial_duty", duty)) setDuty(duty); // boolean pwm enable enable_sub_ = nh_.subscribe("enable", 10, &PwmSysfsDriverNode::enableCallback, this); // boolean pwm invert polarity invert_polarity_sub_ = nh_.subscribe("invert_polarity", 10, &PwmSysfsDriverNode::invertPolarityCallback, this); // pwm cycle period, in nanoseconds period_sub_ = nh_.subscribe("period", 10, &PwmSysfsDriverNode::periodCallback, this); // pwm duty cycle fraction, 0 to 1 duty_sub_ = nh_.subscribe("duty", 10, &PwmSysfsDriverNode::dutyCallback, this); }
void SPIPWMworker(void) { unsigned char token, instruction, fInst=0,fb=0,bf=0; initSPI(); SSP2BUF = 0xFD; enablePWM(); while (1) { while(PORTCbits.RC0 ==0)//CS LOW { if(SSP2STATbits.BF) // we received something :) { bf=1; if(fb==0) { token = SSP2BUF; instruction = token & SPIEEMASK; } fb=1; switch (instruction) { case 0: if(fInst) { if(fInst==1)setPeriod(SSP2BUF); fInst++; } else { fInst=1; } SSP2BUF = PWMperiod; break; case 1: if(fInst) { if(fInst==1)setDuty(SSP2BUF); fInst++; } else { fInst=1; } SSP2BUF = PWMduty; break; } } } if(bf) //CS HIGH { fb=0; bf=0; fInst = 0; } } }
// check tube voltage and adjust duty cycle to match output given threshold level // return false on critical error (when we hit the duty max), true if all ok bool Inverter::adjustDutyCycle(int measuredVoltage) { if (measuredVoltage > highestVoltage) { highestVoltage = measuredVoltage; highestVoltageDuty = duty; } if ( (measuredVoltage >= target + tolerance) && (duty > dutymin)) { setDuty(duty - 1); // we need to decrease duty cycle to decrease voltage } if ( (measuredVoltage <= target - tolerance) && (duty < dutymax)) { setDuty(duty + 1); // we need to increase duty cycle to increase voltage } if (duty == INVERTER_DUTY_MAX) { setDuty(highestVoltageDuty); return false; // error: no place to increase, yet we have not reached the target voltage } else return true; }
int main() { uint8_t i; cli(); wdt_enable(WDTO_60MS); for (i = 0; i < 8; i++) { PORTA = PORTA >> 1; PORTA |= ((readEEPROM(i) - '0') & 0x01) << 7; } for (i = 8; i < 16; i++) { PORTC = PORTC >> 1; PORTC |= ((readEEPROM(i) - '0') & 0x01) << 7; } DDRA = 0xff; DDRC = 0xff; for (stackTail = EEPROM_SIZE - 1; readEEPROM(stackTail) != 0xff; stackTail--); status = ((uint16_t) PORTC) << 8 | (uint16_t) PORTA; initUSART(); setDuty(); initTimer0(); initTimer2(); sei(); printf("\nEntering the main loop\n"); while (1) { wdt_reset(); } return 0; }
void rotate(int time){ if (time > 0){ setDuty(LEFT_WHEEL_INDEX, MAXIMUM_DUTY); setDuty(RIGHT_WHEEL_INDEX, MAXIMUM_DUTY); } else if (time < 0){ setDuty(LEFT_WHEEL_INDEX, MINIMUM_DUTY); setDuty(RIGHT_WHEEL_INDEX, MINIMUM_DUTY); } usleep(microsecondsToMilliseconds(abs(time))); setDuty(LEFT_WHEEL_INDEX, NEUTRAL_DUTY); setDuty(RIGHT_WHEEL_INDEX, NEUTRAL_DUTY); }
void aimAtTarget(double error){ if (error == 0){ return; } if (error < 0){ //Turn left setDuty(LEFT_WHEEL_INDEX, NEUTRAL_DUTY-ADJUST_RATE); setDuty(RIGHT_WHEEL_INDEX, NEUTRAL_DUTY-ADJUST_RATE); } else if (error > 0){ //Turn right setDuty(LEFT_WHEEL_INDEX, NEUTRAL_DUTY+ADJUST_RATE); setDuty(RIGHT_WHEEL_INDEX, NEUTRAL_DUTY+ADJUST_RATE); } setRun(RIGHT_WHEEL_INDEX, 1); setRun(LEFT_WHEEL_INDEX, 1); usleep(1250 * abs(error)); setDuty(LEFT_WHEEL_INDEX, NEUTRAL_DUTY); setDuty(RIGHT_WHEEL_INDEX, NEUTRAL_DUTY); }
void PwmSysfsDriverNode::dutyCallback(std_msgs::Float64::ConstPtr const& msg) { setDuty(msg->data); }
void UARTworker(void) { unsigned char c,mode=0,addr=0,instruction=0,EEaddrF=0,EEaddr=0,adcc=0,helpC; initUART(); //write start message (menu) UARTwriteString(msgMenu[0]); UARTwrite('\n'); while(1) { if(RCIF) { RCIF=0; LED2ON; if(!(RCSTA&0b00000110)) { rhead++; rhead&=RINGBUFFMASK; ringbuff[rhead]=RCREG; } LED2OFF; c=UARTread(); UARTwrite(c); //c=UARTcharFromString(c); switch (mode) { case 0: mode=c-48; UARTwriteString(msgMenu[c-48]); if(mode==2)enablePWM(); else if(mode==3)enableDAC(); break; case 1://ADC switch(c) { case 'r'://single read UARTwriteString("\n\nADC value: "); helpC=getADC(adcc); UARTwriteDecimal(helpC); UARTwriteString(msgMenu[1]); break; case '1'://chanell one UARTwriteString("\n\nchannel 1 selected"); adcc=0; UARTwriteString(msgMenu[1]); break; case '2'://chanel two UARTwriteString("\n\nchannel 2 selected"); adcc=1; UARTwriteString(msgMenu[1]); break; case '3'://chanell three UARTwriteString("\n\nchannel 3 selected"); adcc=2; UARTwriteString(msgMenu[1]); break; case 't'://temp UARTwriteString("\n\nTemp sensor selected"); adcc=3; UARTwriteString(msgMenu[1]); break; case 'm'://back to start mode = 0; UARTwriteString(msgMenu[0]); break; default: break; } break; case 2://PWM if(instruction) { switch(instruction) { case 'p': //pwm period = c; setPeriod(UARTcharFromString(c)); UARTwriteString(msgMenu[2]); break; case 'd': setDuty(UARTcharFromString(c)); UARTwriteString(msgMenu[2]); //pwm period =c; break; case 'm': mode =0; //pwm off UARTwriteString(msgMenu[0]); break; default: break; } instruction = 0; } else { instruction = c; //loads the instruction if(instruction == 'p') { UARTwriteString("\n\nEnter the PWM Period: "); } else if(instruction == 'd') { UARTwriteString("\n\nEnter the PWM Duty Cycle: "); } else if(instruction == 'm') //if it's m goes back to the start menu... { mode =0; instruction =0; disablePWM(); UARTwriteString(msgMenu[0]); } } break; case 3://DAC if(instruction) { switch(instruction) { case 'v': //enter woltage setDAC(UARTcharFromString(c)); UARTwriteString(msgMenu[3]); break; case 'm': mode = 0; UARTwriteString(msgMenu[0]); break; default: break; } instruction =0; } else { instruction = c; //loads the instruction if(instruction == 'v') { UARTwriteString(msgDACsetV); } else if(instruction == 'm') //if it's m goes back to the start menu... { mode =0; instruction =0; disableDAC(); UARTwriteString(msgMenu[0]); } } break; case 4://MEM if(instruction) //if instruction has been sent previusly { if(EEaddrF) //instruction was sent previusly, check if address was sent { //address was sent if(instruction == 'w') //if instruction was W-writes recived character to EEProm[ADDR] { EEPROMwrite(EEaddr,UARTcharFromString(c)); UARTwriteString(msgMenu[4]); //write c to eeprom } else if (instruction == 'r') //if instruction was R-reads EEprom[addr] from eeprom { UARTwriteDecimal(EEPROMread(EEaddr)); UARTwriteString(msgMenu[4]); } else if (instruction == 'm') //if instruction was m --returns to start menu... { mode = 0; UARTwriteString(msgMenu[0]); } EEaddrF=0; //clears the addressing flag instruction =0; //clears the istruction flag } else { EEaddrF=1; //sets the address flage EEaddr=UARTcharFromString(c); if(instruction=='w')UARTwriteString(msgEEw); else if(instruction == 'r')UARTwriteString("\n\nHit any key to read from EEPROM.\n\n"); } } else { instruction = c; //loads the instruction if((instruction == 'w')||(instruction == 'r')) { UARTwriteString(msgEEaddr); } else if(instruction == 'm') //if it's m goes back to the start menu... { mode =0; instruction =0; UARTwriteString(msgMenu[0]); } } break; default: mode=0; UARTwriteString(msgMenu[0]); break; } } } }
void DutyUnit::nr1Change(const unsigned newNr1, const unsigned long cc) { updatePos(cc); setDuty(newNr1); setCounter(); }
/*MAIN*/ int main() { //Declarations int i, j; int targets; int center, error, most_blob; int num_balls = 5; int dist, prev; int n = 0; //Set up GPIO gpio_export(POWER_LED_PIN); gpio_set_dir(POWER_LED_PIN, OUTPUT_PIN); gpio_export(CAMERA_LED_PIN); gpio_set_dir(CAMERA_LED_PIN, OUTPUT_PIN); gpio_export(BUTTON_PIN); gpio_set_dir(BUTTON_PIN, INPUT_PIN); gpio_export(RELOAD_LED_PIN); gpio_set_dir(RELOAD_LED_PIN, OUTPUT_PIN); gpio_export(ARDUINO_PIN); gpio_set_dir(ARDUINO_PIN, OUTPUT_PIN); gpio_set_value(ARDUINO_PIN, LOW); //Set up PWM initPWM(); usleep(microsecondsToMilliseconds(500)); //Dropper setPeriod(DROPPER_MOTOR_INDEX, PWM_PERIOD); setDuty(DROPPER_MOTOR_INDEX, percentageToDuty(DROPPER_DOWN_POSITION)); setRun(DROPPER_MOTOR_INDEX, 1); //Grabber setPeriod(GRABBER_MOTOR_INDEX, PWM_PERIOD); setDuty(GRABBER_MOTOR_INDEX, GRABBER_UP_POSITION); setRun(GRABBER_MOTOR_INDEX, 1); //Right wheel setPeriod(RIGHT_WHEEL_INDEX, PWM_PERIOD); setDuty(RIGHT_WHEEL_INDEX, NEUTRAL_DUTY); setRun(RIGHT_WHEEL_INDEX, 0); //Left wheel setPeriod(LEFT_WHEEL_INDEX, PWM_PERIOD); setDuty(LEFT_WHEEL_INDEX, NEUTRAL_DUTY); setRun(LEFT_WHEEL_INDEX, 0); //Roller setRun(ROLLER_MOTOR_INDEX, 0); setPeriod(ROLLER_MOTOR_INDEX, PWM_PERIOD); setDuty(ROLLER_MOTOR_INDEX, PWM_PERIOD*ROLLER_SPEED); //Set up serial serial* arduinoSerial; if (serial_open(&arduinoSerial, ARDUINO_SERIAL_PORT, ARDUINO_BAUD_RATE) == 0){ printf("Opened Ardunio serial port sucessfully\n"); } else{ printf("Failed to open Arduino serial port\n"); } /*for(i = 0; i < 50; i++) { dist = getDistance(arduinoSerial); printf("Instensity: %d\n", dist); usleep(microsecondsToMilliseconds(100)); } usleep(microsecondsToMilliseconds(5000));*/ //Turn on power LED gpio_set_value(POWER_LED_PIN, HIGH); printf("Systems are GO for launch!\n"); waitForButtonPress(); getDistance(arduinoSerial); //Move to center of the board goForward(); usleep(microsecondsToMilliseconds(1800)); stopWheels(); //Aim at right target rotate(180); if(aimAtSideMostTarget(1)) { for (; num_balls > 3; num_balls--) { fireGolfBall(); } } //Move toward left target rotate(-550); goForward(); usleep(microsecondsToMilliseconds(2200)); stopWheels(); //Aim at left target rotate(300); if(aimAtSideMostTarget(0)) { for (; num_balls > 1; num_balls--) { fireGolfBall(); } } //Aim at middle target rotate(260); aimAtBiggestTarget(); for(; num_balls > 0; num_balls--) { fireGolfBall(); } //Move to sideline for reload rotate(-600); goForward(); buffer_clear(arduinoSerial); do { prev = dist; dist = getDistance(arduinoSerial); //printf("%d, ", dist); usleep(microsecondsToMilliseconds(9)); } while (dist < DISTANCE_TO_WALL || dist == 0 || prev - dist > 5); stopWheels(); rotate(1100); //Turn on reload LED gpio_set_value(RELOAD_LED_PIN, HIGH); usleep(microsecondsToMilliseconds(3000)); gpio_set_value(RELOAD_LED_PIN, LOW); while (n < 3) //big while loop thing! { //start from left num_balls = 5; goForward(); usleep(microsecondsToMilliseconds(1200)); stopWheels(); rotate(-420); goForward(); usleep(microsecondsToMilliseconds(590)); stopWheels(); //Aim at left target if(aimAtSideMostTarget(0)) { for(; num_balls > 3; num_balls--) { fireGolfBall(); } } //Aim at middle target rotate(420); goForward(); usleep(microsecondsToMilliseconds(600)); stopWheels(); rotate(-150); aimAtBiggestTarget(); for(; num_balls > 2; num_balls--) { fireGolfBall(); } //Aim at right target rotate(400); goForward(); usleep(microsecondsToMilliseconds(2700)); stopWheels(); rotate(-370); if(aimAtSideMostTarget(1)) { for(; num_balls > 0; num_balls--) { fireGolfBall(); } } //Move to sideline for reload rotate(320); //usleep(microsecondsToMilliseconds(50)); goForward(); buffer_clear(arduinoSerial); dist = 0; do { prev = dist; dist = getDistance(arduinoSerial); //printf("And another thing....\n"); //printf("%d!!! \n", dist); usleep(microsecondsToMilliseconds(10)); }while (dist < DISTANCE_TO_WALL || dist == 0 || prev - dist > 5); stopWheels(); rotate(-900); gpio_set_value(RELOAD_LED_PIN, HIGH); usleep(microsecondsToMilliseconds(3000)); gpio_set_value(RELOAD_LED_PIN, LOW); //start from right num_balls = 5; goForward(); usleep(microsecondsToMilliseconds(1350)); stopWheels(); rotate(570); //Aim at right target if(aimAtSideMostTarget(1)) { for(; num_balls > 3; num_balls--) { fireGolfBall(); } } //Aim at middle target rotate(-500); goForward(); usleep(microsecondsToMilliseconds(4500)); stopWheels(); rotate(650); aimAtBiggestTarget(); for(; num_balls > 2; num_balls--) { fireGolfBall(); } //Aim at left target rotate(-200); if(aimAtSideMostTarget(0)) { for(; num_balls > 0; num_balls--) { fireGolfBall(); } } //Move to sideline for reload rotate(-400); goForward(); buffer_clear(arduinoSerial); do { prev = dist; dist = getDistance(arduinoSerial); usleep(microsecondsToMilliseconds(9)); } while(dist < DISTANCE_TO_WALL || dist == 0 || prev - dist > 5); stopWheels(); rotate(1000); gpio_set_value(RELOAD_LED_PIN, HIGH); usleep(microsecondsToMilliseconds(3000)); gpio_set_value(RELOAD_LED_PIN, LOW); } //end of while loop //Turn off gpio power gpio_set_value(POWER_LED_PIN, LOW); gpio_set_value(RELOAD_LED_PIN, LOW); gpio_set_value(ARDUINO_PIN, LOW); //Turn off motors setRun(GRABBER_MOTOR_INDEX, 0); setRun(DROPPER_MOTOR_INDEX, 0); setRun(ROLLER_MOTOR_INDEX, 0); setRun(RIGHT_WHEEL_INDEX, 0); setRun(LEFT_WHEEL_INDEX, 0); //Remove GPIO files //gpio_unexport(PING_PIN); return 0; }
void goForward(void){ setRun(LEFT_WHEEL_INDEX, 1); setRun(RIGHT_WHEEL_INDEX, 1); setDuty(LEFT_WHEEL_INDEX, MAXIMUM_DUTY); setDuty(RIGHT_WHEEL_INDEX, MINIMUM_DUTY); }
void stopWheels(void){ setDuty(RIGHT_WHEEL_INDEX, NEUTRAL_DUTY); setDuty(LEFT_WHEEL_INDEX, NEUTRAL_DUTY); }
void liftGolfBall(void){ setDuty(GRABBER_MOTOR_INDEX, GRABBER_UP_POSITION); usleep(microsecondsToMilliseconds(700)); setDuty(GRABBER_MOTOR_INDEX, GRABBER_DOWN_POSITION); usleep(microsecondsToMilliseconds(500)); }
void dropGolfBall(void){ setDuty(DROPPER_MOTOR_INDEX, percentageToDuty(DROPPER_UP_POSITION)); usleep(microsecondsToMilliseconds(400)); setDuty(DROPPER_MOTOR_INDEX, percentageToDuty(DROPPER_DOWN_POSITION)); usleep(microsecondsToMilliseconds(250)); }