main.cpp

00001 #include "mbed.h"
00002 #include "Adafruit_PWMServoDriver.h"
00003 #include "ShapeDetect.h"
00004 
00005 Serial pc(USBTX,USBRX);
00006 Adafruit_PWMServoDriver pwm(p28,p27);
00007 DigitalOut ServoOutputDisable(p8);
00008 extern Serial lrf;
00009 
00010 
00011 
00012 //Servo Positions
00013 #define STORE_POSITION  0
00014 #define OIL_RIG1        1
00015 #define OIL_RIG2        2
00016 #define OIL_RIG3        3
00017 #define IMG_SHAPE_1     4
00018 #define IMG_SHAPE_2     5
00019 #define IMG_SHAPE_3     6
00020 #define GRASP_SHAPE_1   7
00021 #define GRASP_SHAPE_2   8
00022 #define GRASP_SHAPE_3   9
00023 #define INSERT_TOOL_1   10
00024 #define INSERT_TOOL_2   11
00025 #define INSERT_TOOL_3   11
00026 
00027 
00028 void servoBegin(void);
00029 void initServoDriver(void);
00030 void setServoPulse(uint8_t n, float pulse);
00031 void setServoPulseNo_delay(uint8_t n, float pulse);
00032 void servoPosition(int set);
00033 int ServoTest(void);
00034 
00035 /************
00036 Variables for Servos
00037 *************/
00038 int servoNum, pulseWidth, outputDisabled, posNum;
00039 int currentPosition[7]; 
00040 
00041 typedef struct {int arm_action; int base_rotate; int base_arm; 
00042                 int lil_arm; int big_arm; int claw_arm; 
00043                 int claw_rotate; int claw_open;} Coord;
00044  
00045 Coord Arm_Table[] = 
00046 {
00047    // POSITION ODER:
00048    // base_rotate, base_arm, lil_arm, int big_arm, int claw_arm, int claw_rotate, int claw_open
00049       
00050     {STORE_POSITION, 46, 0, 0, 100, 96, 0, 0}, // storing position
00051     {OIL_RIG1, 83, 75, 100, 33, 33, 0, 0}, // point laser at oilrig2
00052     {STORE_POSITION, 100, 17, 17, 0, 100, 0, 0}, // Shape Detect
00053     {STORE_POSITION, 900, 500, 600, 2450, 2450, 0, 0}, // storing position
00054     {STORE_POSITION, 900, 500, 600, 2450, 2450, 0, 0}, // storing position
00055     {STORE_POSITION, 900, 500, 600, 2450, 2450, 0, 0}, // storing position
00056     {STORE_POSITION, 900, 500, 600, 2450, 2450, 0, 0}, // storing position
00057 
00058 }; 
00059 
00060 int main() {
00061     
00062       
00063 /*****************
00064  INITIALIZATIONS
00065 *******************/
00066 //pc.baud(115200); 
00067 //Laser Range Finder Initialization
00068 //lrf_baudCalibration();
00069  
00070 //Servo initialization
00071 initServoDriver();
00072 servoBegin(); // initiates servos to start position
00073 ServoOutputDisable = 0;
00074 
00075 while(1){
00076     int instr;
00077         
00078     pc.printf("Set Servo Position = 0, Shape Detect = 1, Oil Rig Detect = 2");
00079     while(!pc.readable());
00080     pc.scanf("%d", &instr);
00081     
00082     if(instr == 0){// Servo Control
00083         while(!ServoTest());
00084     }
00085     else if( instr == 1){// shape detect 
00086         //servoPosition(3);
00087         
00088         
00089     }
00090     else if( instr == 2){// oil rig detect
00091             servoPosition(2);
00092             
00093     }
00094 
00095 }// End Main while        
00096 }
00097 
00098 /**************************************************
00099 *     FIRST STAGE 
00100 *               
00101 *          - DETERMINE OIL RIG ON FIRE
00102 *          - DETERMINE PATH
00103 *   
00104 **************************************************/
00105     
00106     //TODO: EXTEND ARM AND FACE OILRIGS
00107     
00108     //OILRIG 1 DISTANCE READING
00109 
00110     //TODO: ROTATE ARM TO NEXT OIL RIG
00111     
00112     //OILRIG 2 DISTANCE READING  
00113     
00114     //ROTATE ARM TO NEXT OIL RIG
00115     
00116     //OILRIG 3 DISTANCE READING
00117 
00118 
00119 
00120 
00121 
00122 /************
00123 
00124 Servo Functions
00125 
00126 **************/
00127 
00128 void setServoPulse(uint8_t n, float pulse) {
00129     float pulselength = 20000;   // 20,000 us per second
00130     int i = currentPosition[n], pstart;
00131     int pulse2, min=500, max=2700; 
00132     
00133     // Set when to start the pulse for the different servos
00134     // and normalize values 
00135  /*   if(n < 2){
00136         pstart = 0;// need to change this to match the different servos
00137         pulse1= pulse ;
00138     }    
00139     else{
00140         pstart = 0;
00141         pulse1=pulse;     
00142     } */   
00143     
00144     pulse= ((pulse*(max-min))/100)+min;
00145     
00146     if(currentPosition[n] < pulse){
00147           pc.printf("\ncurrent position < pulse\n\r");
00148           for(i=currentPosition[n]; i < pulse; i++){  
00149           pulse2 = 4094 * i / pulselength;
00150           pwm.setPWM(n, pstart, pulse2);
00151           wait_ms(1); 
00152           }
00153     } else if (currentPosition[n] > pulse) {
00154            pc.printf("\ncurrent position > pulse\n\r");
00155            for(i=currentPosition[n]; i > pulse; i--){  
00156            pulse2 = 4094 * i / pulselength;
00157            pwm.setPWM(n, pstart, pulse2);
00158            wait_ms(1);
00159            }
00160     }
00161     currentPosition[n] = i;
00162     pc.printf("\nending position = %d\n\n\r", i);  
00163 }
00164 
00165 void initServoDriver(void) {
00166     pwm.begin();
00167     //pwm.setPWMFreq(100);  //This dosen't work well because of uncertain clock speed. Use setPrescale().
00168     pwm.setPrescale(140);    //This value is decided for 20ms interval.
00169     pwm.setI2Cfreq(400000); //400kHz
00170     
00171 }
00172 
00173 void servoBegin(void){
00174     setServoPulseNo_delay(0, 46);
00175     setServoPulseNo_delay(1, 0);
00176     setServoPulseNo_delay(2, 0);
00177     setServoPulseNo_delay(3, 100);
00178     setServoPulseNo_delay(4, 96); 
00179     setServoPulseNo_delay(5, 0);
00180     setServoPulseNo_delay(6, 0); 
00181     }
00182     
00183 void setServoPulseNo_delay(uint8_t n, float pulse) {
00184     float pulselength = 20000;   // 20,000 us per second
00185     int min=500, max=2700; 
00186     
00187     pulse= ((pulse*(max-min))/100)+min;
00188     currentPosition[n] = pulse;
00189     pulse = 4094 * pulse / pulselength;
00190     pwm.setPWM(n, 0, pulse);
00191 }
00192     
00193 void servoPosition(int set){   
00194             //moves to current position
00195             setServoPulse(0, Arm_Table[set].base_rotate);
00196             setServoPulse(1, Arm_Table[set].base_arm);
00197             setServoPulse(2, Arm_Table[set].lil_arm);
00198             setServoPulse(3, Arm_Table[set].big_arm);
00199             setServoPulse(4, Arm_Table[set].claw_arm); 
00200             setServoPulse(5, Arm_Table[set].claw_rotate); 
00201             setServoPulse(6, Arm_Table[set].claw_open); 
00202 }
00203 
00204 int ServoTest(void){
00205     int num, delta=1;  // Change Delta to  change increment size
00206     static int move[7]={46, 0, 0, 100, 96, 0, 0};
00207     
00208     if(pc.readable()){
00209         num=pc.getc();
00210              
00211         if(num == 'a' || num == 'z'){
00212                 if(move[0]>=(100-delta))move[0]=(100-delta);
00213                 else if(move[0] <= (0+delta)) move[0]=delta;
00214                  
00215                 if(num=='a')setServoPulse(0, (move[0]+=delta));
00216                 else setServoPulse(0, (move[0]-=delta));
00217         }
00218         else if(num == 's' || num == 'x'){
00219                 if(move[1]>=(100-delta))move[1]=(100-delta);
00220                 else if(move[1] <= (0+delta)) move[1]=delta;
00221                  
00222                 if(num=='s')setServoPulse(1, (move[1]+=delta));
00223                 else setServoPulse(1, (move[1]-=delta));
00224         }
00225         else if(num == 'd' || num == 'c'){
00226                 if(move[2]>=(100-delta))move[2]=(100-delta);
00227                 else if(move[2] <= (0+delta)) move[2]=delta;
00228                  
00229                 if(num=='d')setServoPulse(2, (move[2]+=delta));
00230                 else setServoPulse(2, (move[2]-=delta));
00231         }
00232         else if(num == 'f' || num == 'v'){
00233                 if(move[3]>=(100-delta))move[3]=(100-delta);
00234                 else if(move[3] <= (0+delta)) move[3]=delta;
00235                  
00236                 if(num=='f')setServoPulse(3, (move[3]+=delta));
00237                 else setServoPulse(3, (move[3]-=delta));
00238         }
00239         else if(num == 'g' || num == 'b'){
00240                 if(move[4]>=(100-delta))move[4]=(100-delta);
00241                 else if(move[4] <= (0+delta)) move[4]=delta;
00242                  
00243                 if(num=='g')setServoPulse(4, (move[4]+=delta));
00244                 else setServoPulse(4, (move[4]-=delta));
00245         }
00246         else if(num == 'h' || num == 'n'){
00247                 if(move[5]>=(100-delta))move[5]=(100-delta);
00248                 else if(move[5] <= (0+delta)) move[5]=delta;
00249                  
00250                 if(num=='h')setServoPulse(5, (move[5]+=delta));
00251                 else setServoPulse(5, (move[5]-=delta));
00252         }
00253         else if(num == 'j' || num == 'n'){
00254                 if(move[6]>=(100-delta))move[6]=(100-delta);
00255                 else if(move[6] <= (0+delta)) move[6]=delta;
00256                  
00257                 if(num=='j')setServoPulse(6, (move[6]+=delta));
00258                 else setServoPulse(6, (move[6]-=delta));
00259         }  
00260         else if(num== 'e') return 1;
00261                 
00262         pc.printf("0\t 1\t 2\t 3 \t4 \t 5 \t 6\t type e to end\n\r");
00263         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]);
00264     }
00265     return 0;
00266 }
00267