added normalization and use of keyboard. I tested it and it worked
Dependencies: Adafruit-16-Ch-PWM-Servo-Driver mbed
Fork of theRobot by
Diff: main.cpp
- Revision:
- 3:587441455259
- Parent:
- 2:4e082e4c255d
- Child:
- 4:116829a5ae3c
diff -r 4e082e4c255d -r 587441455259 main.cpp --- a/main.cpp Mon Mar 03 22:30:34 2014 +0000 +++ b/main.cpp Thu Mar 06 00:12:56 2014 +0000 @@ -30,6 +30,7 @@ void setServoPulse(uint8_t n, float pulse); void setServoPulseNo_delay(uint8_t n, float pulse); void servoPosition(int set); +int ServoTest(void); /************ Variables for Servos @@ -48,7 +49,7 @@ {STORE_POSITION, 900, 500, 600, 2450, 2450, 0, 0}, // storing position {OIL_RIG1, 1500, 1400, 1900, 900, 900, 0, 0}, // point laser at oilrig2 - {STORE_POSITION, 900, 500, 600, 2450, 2450, 0, 0}, // storing position + {STORE_POSITION, 2350, 1050, 700, 500, 2350, 0, 0}, // Shape Detect {STORE_POSITION, 900, 500, 600, 2450, 2450, 0, 0}, // storing position {STORE_POSITION, 900, 500, 600, 2450, 2450, 0, 0}, // storing position {STORE_POSITION, 900, 500, 600, 2450, 2450, 0, 0}, // storing position @@ -58,6 +59,7 @@ int main() { + /***************** INITIALIZATIONS *******************/ @@ -71,16 +73,26 @@ ServoOutputDisable = 0; while(1){ - if(pc.readable()){ - /* - pc.scanf("%d %d %d", &outputDisabled, &servoNum, &pulseWidth); - setServoPulse(servoNum, pulseWidth); - */ - pc.scanf("%d", &posNum); - servoPosition(posNum); + int instr; + + printf("Set Servo Position = 0, Shape Detect = 1, Oil Rig Detect = 2"); + while(!pc.readable()); + pc.scanf("%d", &instr); + + if(instr == 0){// Servo Control + while(!ServoTest()); + } + else if( instr == 1){// shape detect + //servoPosition(3); + + + } + else if( instr == 2){// oil rig detect + servoPosition(2); - - } + } + +}// End Main while } /************************************************** @@ -103,7 +115,7 @@ //OILRIG 3 DISTANCE READING -} + @@ -115,21 +127,33 @@ void setServoPulse(uint8_t n, float pulse) { float pulselength = 20000; // 20,000 us per second - int i = currentPosition[n]; + int i = currentPosition[n], pstart; pc.printf("\ncurrent position = %d\n", currentPosition[n]); - int pulse2; + int pulse2, pulse1; + + // Set when to start the pulse for the different servos + // and normalize values + if(n < 2){ + pstart = 0;// need to change this to match the different servos + pulse1= pulse + 500; + } + else{ + pstart = 0; + pulse1=pulse+500; + } + if(currentPosition[n] < pulse){ pc.printf("\ncurrent position < pulse\n"); - for(i; i < pulse; i++){ + for(i; i < pulse1; i++){ pulse2 = 4094 * i / pulselength; - pwm.setPWM(n, 0, pulse2); + pwm.setPWM(n, pstart, pulse2); wait_ms(3); } } else if (currentPosition[n] > pulse) { pc.printf("\ncurrent position > pulse\n"); - for(i; i > pulse; i--){ + for(i; i > pulse1; i--){ pulse2 = 4094 * i / pulselength; - pwm.setPWM(n, 0, pulse2); + pwm.setPWM(n, pstart, pulse2); wait_ms(3); } } @@ -173,5 +197,67 @@ setServoPulse(6, Arm_Table[set].claw_open); } +int ServoTest(void){ + int num, delta=50; // Change Delta to change increment size + static int move[7]={900, 500, 600, 2450, 2450, 0, 0}; + + if(pc.readable()){ + num=pc.getc(); + + if(num == 'a' || num == 'z'){ + if(move[0]>2650)move[0]=2700; + else if(move[0] < 550) move[0]=500; + + if(num=='a')setServoPulse(0, (move[0]+=delta)); + else setServoPulse(0, (move[0]-=delta)); + } + else if(num == 's' || num == 'x'){ + if(move[1]> 2650)move[1]=2700; + else if(move[1] < 550) move[1]=500; + + if(num=='s')setServoPulse(1, (move[1]+=delta)); + else setServoPulse(1, (move[1]-=delta)); + } + else if(num == 'd' || num == 'c'){ + if(move[2]> 2650)move[2]=2700; + else if(move[2] < 550) move[2]=500; + + if(num=='d')setServoPulse(2, (move[2]+=delta)); + else setServoPulse(2, (move[2]-=delta)); + } + else if(num == 'f' || num == 'v'){ + if(move[3]> 2650)move[3]=2700; + else if(move[3] < 550) move[3]=500; + + if(num=='f')setServoPulse(3, (move[3]+=delta)); + else setServoPulse(3, (move[3]-=delta)); + } + else if(num == 'g' || num == 'b'){ + if(move[4]> 2650)move[4]=2700; + else if(move[4] < 550) move[4]=500; + + if(num=='g')setServoPulse(4, (move[4]+=delta)); + else setServoPulse(4, (move[4]-=delta)); + } + else if(num == 'h' || num == 'n'){ + if(move[5]> 2650)move[5]=2700; + else if(move[5] < 550) move[5]=500; + + if(num=='h')setServoPulse(5, (move[5]+=delta)); + else setServoPulse(5, (move[5]-=delta)); + } + else if(num == 'j' || num == 'n'){ + if(move[6]> 2650)move[6]=2700; + else if(move[6] < 550) move[6]=500; + + if(num=='j')setServoPulse(6, (move[6]+=delta)); + else setServoPulse(6, (move[6]-=delta)); + } + else if(num== 'e') return 1; + + pc.printf("0\t 1\t 2\t 3 \t4 \t 5 \t 6\t type e to end\n\r"); + pc.printf("%d\t %d\t %d\t %d \t%d \t %d \t %d\n\r",move[0],move[1], move[2], move[3],move[4],move[5],move[6]); + } + return 0; +} -