Exemple #1
0
/*****************************************************************************
* Function Name : test_motor
* Description   : Turns on a specified motor for a specified period of time
*                 and duty cycle
* Parameters    : type - The type field of the motor test packet
*                 status - Status field of the motor test packet (not yet used)
*                 length - The length of the payload data array
*                 data - data[0] = motor number
*                        data[1:2] = on time (milli secs)
*                        data[3:4] = duty cycle (percent)
* Return Value  : success indicator - 0 for failed, 1 for succeeded
*****************************************************************************/
unsigned char test_motor(unsigned char type, unsigned char status, \
                         unsigned char length, unsigned char* data)
{   unsigned int motor_id;
    int on_time;
    int dutycycle;
    char ack_string[40]="motor OK\n";
    MacPacket packet;
    Payload pld;

    motor_id = (unsigned int) data[0];
    on_time = (unsigned long)( (data[3] << 8) + data[2]);
    dutycycle = (int)((data[5] << 8) + data[4]);
    tiHSetDC(motor_id, dutycycle);
    swatchDelayMs(on_time);
    tiHSetDC(motor_id, 0);
// send an ack packet back - could have data later...
    // Get a new packet from the pool
    packet = radioRequestPacket(sizeof(ack_string));
    if(packet == NULL) return 0;
    //macSetDestAddr(packet, RADIO_DEST_ADDR);

    // Prepare the payload
    pld = packet->payload;
    paySetStatus(pld, STATUS_UNUSED);
    paySetType(pld, type);
// Read message data into the payload
    memcpy(payGetData(pld),  & ack_string, sizeof(ack_string)); // copy ack_string to packet

    // Enqueue the packet for broadcast
    radioEnqueueTxPacket(packet);

    return 1; //success
}
Exemple #2
0
void tiHSetup(void) {

    //This setup is unique to PWM_IPCLK_SCALE1
    //If the clock scaler is changed, this MUST be changed too!
    pwm_period = (int)((float)FCY/((float)PWM_FREQ * 1.0))- 1;

    sevtcmp_pwm = (int)(ADC_TRIG_POINT * (float)pwm_period);

    max_pwm = 2*sevtcmp_pwm;

    tiHSetupPeripheral();
    int i;
    for(i=0; i< NUM_PWM; i++) {
        outputs[i].throt_f = 0.0;
        outputs[i].throt_i = 0;
        outputs[i].mode = TIH_MODE_COAST;
        outputs[i].dir = TIH_FWD;
    }

    //Start all channels at 0 throttle in Forward / Coast mode
    tiHSetDC(1,0);
    tiHSetDC(2,0);
    tiHSetDC(3,0);
    tiHSetDC(4,0);
}
Exemple #3
0
// set PWM values for short duration for each motor
// throttle[0], throttle[1], duration
void cmdSetThrust(unsigned char type, unsigned char status, unsigned char length, unsigned char *frame)
 {	int thrust1 = frame[0] + (frame[1] << 8);
	int thrust2 = frame[2] + (frame[3] << 8);
	unsigned int run_time_ms = frame[4] + (frame[5] << 8);

	DisableIntT1;	// since PID interrupt overwrites PWM values
// wiring was scrambled Jan. 9, 2013 on RSF velociRoACH robot
  	tiHSetDC(1, -thrust2);
	tiHSetDC(2, thrust1); 
	delay_ms(run_time_ms);
	tiHSetDC(1,0);
	tiHSetDC(2,0);
	EnableIntT1;
 }  
Exemple #4
0
unsigned char cmdSetThrustOpenLoop(unsigned char type, unsigned char status, unsigned char length, unsigned char *frame) {
    int thrust1 = frame[0] + (frame[1] << 8);
    int thrust2 = frame[2] + (frame[3] << 8);
    unsigned int run_time_ms = frame[4] + (frame[5] << 8);

    DisableIntT1;   // since PID interrupt overwrites PWM values

    tiHSetDC(1, thrust1);
    tiHSetDC(2, thrust2);
    delay_ms(run_time_ms);
    tiHSetDC(1,0);
    tiHSetDC(2,0);

    EnableIntT1;
    return 1;
 } 
Exemple #5
0
void pidSetControl()
{ int j;
// 0 = right side
    for(j=0; j < NUM_PIDS; j++)
   {  //pidobjs[0] : right side
	// p_input has scaled velocity interpolation to make smoother
	// p_state is [16].[16]
        	pidObjs[j].p_error = pidObjs[j].p_input + pidObjs[j].interpolate  - pidObjs[j].p_state;
            pidObjs[j].v_error = pidObjs[j].v_input - pidObjs[j].v_state;  // v_input should be revs/sec
            //Update values
            UpdatePID(&(pidObjs[j]));
       } // end of for(j)

		if(pidObjs[0].onoff && pidObjs[1].onoff)  // both motors on to run
		{
 		   tiHSetDC(1, pidObjs[0].output); 
		   tiHSetDC(2, pidObjs[1].output); 
		} 
		else // turn off motors if PID loop is off
		{ tiHSetDC(1,0); tiHSetDC(2,0); }	
}