uyvug
Dependencies: Adafruit-16-Ch-PWM-Servo-Driver HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Fork of theRobot by
Diff: main.cpp
- Revision:
- 1:fe4a0b47ff25
- Parent:
- 0:1b64a0cedc5d
- Child:
- 2:4e082e4c255d
diff -r 1b64a0cedc5d -r fe4a0b47ff25 main.cpp --- a/main.cpp Mon Mar 03 15:27:32 2014 +0000 +++ b/main.cpp Mon Mar 03 21:37:24 2014 +0000 @@ -14,9 +14,10 @@ #define OIL_RIG1 1 #define OIL_RIG2 2 -void restingState(void); +void servoBegin(void); void initServoDriver(void); void setServoPulse(uint8_t n, float pulse); +void setServoPulseNo_delay(uint8_t n, float pulse); void servoPosition(int set); /************ @@ -46,16 +47,15 @@ *******************/ //lrf_baudCalibration(); initServoDriver(); -restingState(); -servoPosition(STORE_POSITION); -ServoOutputDisable = 0; +servoBegin(); +//servoPosition(STORE_POSITION); +//ServoOutputDisable = 0; while(1){ if(pc.readable()){ - pc.scanf("%d %d %d", &outputDisabled, &servoNum, &pulseWidth); - //pc.scanf("%d", &posNum); - //servoPosition(posNum); + //pc.scanf("%d %d %d", &outputDisabled, &servoNum, &pulseWidth); + pc.scanf("%d %d", &servoNum, &pulseWidth); setServoPulse(servoNum, pulseWidth); - ServoOutputDisable = outputDisabled; + } } @@ -91,16 +91,27 @@ **************/ void setServoPulse(uint8_t n, float pulse) { - /* float pulselength = 20000; // 20,000 us per second - if (currentPosition[n] < pulse){ - for (int i=currentPosition[n]; i<(pulse+1); i++) - pulse = 4094 * currentPosition[n]+1 / pulselength; - pwm.setPWM(n, 0, pulse); - } else { - }*/ - float pulselength = 20000; // 20,000 us per second - pulse = 4094 * currentPosition[n]+1 / pulselength; - pwm.setPWM(n, 0, pulse); + float pulselength = 20000; // 20,000 us per second + int i = currentPosition[n]; + pc.printf("\ncurrent position = %d\n", currentPosition[n]); + int pulse2; + if(currentPosition[n] < pulse){ + pc.printf("\ncurrent position < pulse\n"); + for(i; i < pulse; i++){ + pulse2 = 4094 * i / pulselength; + pwm.setPWM(n, 0, pulse2); + wait_ms(3); + } + } else if (currentPosition[n] > pulse) { + pc.printf("\ncurrent position > pulse\n"); + for(i; i > pulse; i--){ + pulse2 = 4094 * i / pulselength; + pwm.setPWM(n, 0, pulse2); + wait_ms(3); + } + } + currentPosition[n] = i; + pc.printf("\nending position = %d\n\n", i); } void initServoDriver(void) { @@ -108,29 +119,29 @@ //pwm.setPWMFreq(100); //This dosen't work well because of uncertain clock speed. Use setPrescale(). pwm.setPrescale(140); //This value is decided for 20ms interval. pwm.setI2Cfreq(400000); //400kHz + } -void restingState(void){ - /* - currentPosition[0] = 2300; - currentPosition[1] = 500; - currentPosition[2] = 600; - currentPosition[3] = 2450; - currentPosition[4] = 2450; - currentPosition[5] = 0; - currentPosition[6] = 0; - */ +void servoBegin(void){ + setServoPulseNo_delay(0, 900); + setServoPulseNo_delay(1, 500); + setServoPulseNo_delay(2, 600); + setServoPulseNo_delay(3, 2450); + setServoPulseNo_delay(4, 2450); + setServoPulseNo_delay(5, 0); + setServoPulseNo_delay(6, 0); + } - setServoPulse(0, 2300); - setServoPulse(1, 500); - setServoPulse(2, 600); - setServoPulse(3, 2450); - setServoPulse(4, 2450); - } +void setServoPulseNo_delay(uint8_t n, float pulse) { + float pulselength = 20000; // 20,000 us per second + currentPosition[n] = pulse; + pulse = 4094 * pulse / pulselength; + pwm.setPWM(n, 0, pulse); +} void servoPosition(int set){ //moves to current position - setServoPulse(1, Arm_Table[set].base_arm,); + setServoPulse(1, Arm_Table[set].base_arm); setServoPulse(0, Arm_Table[set].base_rotate); setServoPulse(2, Arm_Table[set].lil_arm); setServoPulse(3, Arm_Table[set].big_arm);