Revision 0:1b64a0cedc5d, committed 2014-03-03

Comitter:

tashworth

Date:

Mon Mar 03 15:27:32 2014 +0000

Child:

1:fe4a0b47ff25

Commit message:

servo fix, and servo positions

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Adafruit-PWM-Servo-Driver.lib	Mon Mar 03 15:27:32 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/tashworth/code/Adafruit-16-Ch-PWM-Servo-Driver/#02a963fc3700

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ShapeDetect.cpp	Mon Mar 03 15:27:32 2014 +0000
@@ -0,0 +1,308 @@
+#include "mbed.h"
+#include "ShapeDetect.h"
+
+LocalFileSystem local("local");   
+DigitalOut myled(LED1);
+Serial lrf(p9,p10);
+extern Serial pc;
+
+
+/* variables used */
+unsigned char image[120][160];                
+unsigned char pixel;                          
+char lrfchar=0;                                 
+unsigned int s_pixel[8];
+int num_edges = 0, i=0, j=0;
+int mm_range=0;
+
+int s_pixel_sum = 0;
+char min;
+char max;
+
+int xcoord_val;
+int ycoord_val; 
+int s_area_val;
+
+
+/**********************************************
+*    
+*   Initializes the LRF
+*   
+***********************************************/
+
+void lrf_baudCalibration(void){
+    wait(2.5);
+    lrf.baud(115200);  
+    do {
+        lrf.putc('U');
+        pc.putc('.');
+        wait(.2);
+        if (lrf.readable()) lrfchar = lrf.getc();
+    } while (lrfchar != ':');
+    pc.printf("\n\r");
+    // clear out any extra characters - just in case
+    while (lrf.readable()) {
+        lrfchar = lrf.getc();
+    }
+}
+
+
+
+/**********************************************
+*   Turns all pixels to a value and stores in 
+*          image array
+*   
+*    input arrayType_a:
+*           1 = BINARY  ( 1 or 0 ) 
+*           2 = GREYSCALE (greyscale value  0x00 to 0xFF)
+*   
+***********************************************/
+void ImageToArray(int arrayType_a){
+    lrf.putc('G');
+    pc.printf("Capture Image\n\r");
+    // Fill the data with values
+    switch(arrayType_a){
+        
+        case 1:
+            for(int i=0; i<120; i++){
+                 for(int j=0; j<160; j++){
+                     pixel = lrf.getc();
+                     if (pixel < THRESHOLD){
+                      image[i][j] = 1;
+                      } else {image[i][j] = 0;}
+                    }// end j for loop
+                 }// end i for loop
+        break;
+        
+        case 2:
+        for(int i=0; i<120; i++){
+                 for(int j=0; j<160; j++){
+                      image[i][j] = lrf.getc();
+                      }// j
+                 }// i
+        break;
+        
+        default:
+            for(int i=0; i<120; i++){
+                 for(int j=0; j<160; j++){
+                     pixel = lrf.getc();
+                     if (pixel < THRESHOLD){
+                      image[i][j] = 1;
+                      } else {image[i][j] = 0;}
+                    }// j
+                 }// i
+        }//switch
+}
+    
+    
+/***************************************************
+*   prints 160x120 image array to files (.txt & .dat)
+*   
+*   stores on the mbed locally
+*
+*   input arrayType_f:
+*           1 = BINARY  (1 or 0)
+*           2 = GREYSCALE (greyscale value  0x00 to 0xFF)
+*           3 = DECIMAL   (greyscale value 0 - 255)
+*
+****************************************************/
+
+
+void printImageToFile(int arrayType_f){
+ 
+    // Write to local files
+    FILE *fp = fopen("/local/image.dat", "w");  
+    FILE *fp2 = fopen("/local/image.txt", "w"); 
+    
+    for (int i=0; i<120; i++) {
+        for(int j=0; j<160; j++){
+                 switch (arrayType_f){
+                     
+                     //BINARY OUTPUT
+                     case 1:
+                        fprintf(fp, "%d",image[i][j]);
+                        fprintf(fp2, "%d",image[i][j]);
+                        break;
+                     
+                     //GREYSCALE OUTPUT  00 to FF
+                     case 2:
+                        fprintf(fp, "%02X ",image[i][j]); 
+                        fprintf(fp2, "%02X ",image[i][j]); 
+                        break;
+                     
+                     //DECIMAL OUTPUT   
+                     case 3:
+                        fprintf(fp, "%3d ",image[i][j]);
+                        fprintf(fp2, "%3d ",image[i][j]);
+                        break;
+                    
+                    default:
+                        fprintf(fp, "%02h",image[i][j]);
+                        fprintf(fp2, "%02h",image[i][j]);
+                        break;
+                 }
+        }
+        fprintf(fp, "\n");
+        fprintf(fp2, "\n");
+    }
+    fclose(fp);
+}
+
+   
+/**********************************************
+*   edgeDetection 
+*   
+*   outputs the number of edges detected 
+*
+*
+***********************************************/
+int edgeDetection(void){  
+
+    ImageToArray(BINARY);
+
+    for(int i=0; i<120; i++){
+                 for(int j=0; j<160; j++){
+                     s_pixel_sum = 0;
+                     //stores the value of 8 surrounding pixels
+                     s_pixel[0] = image[i-1][j-1];
+                     s_pixel[1] = image[i-1][j];
+                     s_pixel[2] = image[i-1][j+1];
+                     s_pixel[3] = image[i][j-1];
+                     s_pixel[4] = image[i][j+1];
+                     s_pixel[5] = image[i+1][j-1];
+                     s_pixel[6] = image[i+1][j];
+                     s_pixel[7] = image[i+1][j+1];
+                     
+                     //get the sum of the pixels
+                     for (int k=0; k<8; k++){
+                         s_pixel_sum+=s_pixel[k];
+                        }
+                        
+                     if ((s_pixel_sum < 5) && (s_pixel_sum > 2)) {
+                         image[i][j] = 1;
+                         } else { 
+                         image[i][j] = 0;
+                         }
+                     
+                         
+                     }// for j for loop
+                 }// end i for loop
+                 //fill image array with buffer values
+                
+                    
+                 
+                 /// DO EDGE CALCULATION HERE
+                 return num_edges;    
+} // end function
+
+
+
+/**********************************************
+*   center of mass coordinates
+*   
+*   outputs the coordinmates  
+*
+***********************************************/
+
+void centerMass(int *xcoord, int *ycoord, int *s_area){
+    ImageToArray(BINARY);
+    clearBounds();
+    int count = 0;
+    int sumi = 0;
+    int sumj = 0;
+    for(int i=0; i<120; i++){
+                 for(int j=0; j<160; j++){
+                     if(image[i][j] == 1){
+                     sumi += i;
+                     sumj += j;
+                     count++;
+                     }
+                    }// j
+                 }// i          
+    
+    int x_2coord = sumj / count;
+    int y_2coord = sumi / count;
+    *xcoord = x_2coord;
+    *ycoord = y_2coord;
+    image[x_2coord][x_2coord] = 8;
+    
+    int s_2area = 0;
+    for(int i=0; i<120; i++){
+                 for(int j=0; j<160; j++){
+                     if ( image[i][j] == 1 ){
+                        s_2area++;
+                        } 
+                     
+                    }// j
+                 }// i
+    *s_area = s_2area;
+}
+/***********************************************
+*
+*   clears edges for more accurate area counting
+*
+*
+************************************************/
+void clearBounds(void){
+        //top 20 pixels
+        for(int i=0; i<10; i++){
+                 for(int j=0; j<160; j++){
+                     image[i][j] = 0;
+                        
+                    }// j
+                 }// i
+                 
+        //bottom 20 pixels         
+        for(int i=109; i<120; i++){
+                 for(int j=0; j<160; j++){
+                     image[i][j] = 0;        
+                    }// j
+                 }// i
+        //left 20 pixels         
+        for(int i=0; i<120; i++){
+                 for(int j=0; j<10; j++){
+                     image[i][j] = 0;     
+                    }// j
+                 }// i
+         //right 20 pixels         
+        for(int i=0; i<120; i++){
+                 for(int j=149; j<160; j++){
+                     image[i][j] = 0;    
+                    }// j
+                 }// i
+       //clear bounds of detected blacks           
+}   
+
+int shapeDetection_mass(void){
+    
+    centerMass(&xcoord_val, &ycoord_val, &s_area_val);
+    if(s_area_val > 2800){
+        pc.printf("\nSQUARE DETECTECD\n\r");
+        return 3;
+        } else if (s_area_val < 2200){
+            pc.printf("\nTRIANGLE DETECTECD\n\r");
+            return 1;
+            }else{
+                pc.printf("\nCIRCLE DETECTECD\n\r");
+                return 2;
+                }
+    
+    }
+int laserDistance(void){
+        myled=1;
+        lrf.putc('B'); //Take Binary range reading
+        // read in the four bytes for the range in mm (MSB first)
+        mm_range=0;
+        mm_range=lrf.getc();
+        mm_range=(mm_range<<8)|lrf.getc();
+        mm_range=(mm_range<<8)|lrf.getc();
+        mm_range=(mm_range<<8)|lrf.getc();
+        myled=0;
+        //eat CR & ":" command prompt
+        do {
+            lrfchar=lrf.getc();
+        } while (lrfchar != ':');
+        return 1;
+}
+    
+    

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ShapeDetect.h	Mon Mar 03 15:27:32 2014 +0000
@@ -0,0 +1,24 @@
+#ifndef SHAPEDETECT_H_
+#define SHAPEDETECT_H_
+
+/* theshold for setting binary output */
+#define THRESHOLD 100        
+
+/* modes for image processing */
+#define BINARY    1         
+#define GREYSCALE 2
+#define DECIMAL   3
+
+
+ 
+void lrf_baudCalibration(void);
+void printImageToFile(int arrayType_f);
+int edgeDetection(void); 
+void ImageToArray(int arrayType_a);
+void centerMass(int *xcoord, int *ycoord, int *s_area);
+void clearBounds(void);
+int shapeDetection_mass(void);
+int laserDistance(void);
+
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Mar 03 15:27:32 2014 +0000
@@ -0,0 +1,143 @@
+#include "mbed.h"
+#include "Adafruit_PWMServoDriver.h"
+#include "ShapeDetect.h"
+
+Serial pc(USBTX,USBRX);
+Adafruit_PWMServoDriver pwm(p9,p10);
+DigitalOut ServoOutputDisable(p8);
+extern Serial lrf;
+
+
+
+//Servo Positions
+#define STORE_POSITION  0
+#define OIL_RIG1        1
+#define OIL_RIG2        2
+
+void restingState(void);
+void initServoDriver(void);
+void setServoPulse(uint8_t n, float pulse);
+void servoPosition(int set);
+
+/************
+Variables for Servos
+*************/
+int servoNum, pulseWidth, outputDisabled, posNum;
+int currentPosition[7]; 
+
+typedef struct {int arm_action; int base_rotate; int base_arm; 
+                int lil_arm; int big_arm; int claw_arm; 
+                int claw_rotate; int claw_open;} Coord;
+ 
+Coord Arm_Table[] = 
+{
+   // POSITION ODER:
+   // base_rotate, base_arm, lil_arm, int big_arm, int claw_arm, int claw_rotate, int claw_open
+      
+    {STORE_POSITION, 2300, 500, 600, 2450, 2450, 0, 0}, // storing position
+    {OIL_RIG2, 1200, 1400, 1900, 900, 900, 0, 0} // point laser at oilrig2
+
+}; 
+
+int main() {
+    
+/*****************
+ INITIALIZATIONS
+*******************/
+//lrf_baudCalibration();
+initServoDriver();
+restingState();
+servoPosition(STORE_POSITION);
+ServoOutputDisable = 0;
+while(1){
+        if(pc.readable()){
+            pc.scanf("%d %d %d", &outputDisabled, &servoNum, &pulseWidth);
+            //pc.scanf("%d", &posNum);
+            //servoPosition(posNum);
+             setServoPulse(servoNum, pulseWidth);
+             ServoOutputDisable = outputDisabled;
+        }
+}
+
+
+/**************************************************
+*     FIRST STAGE 
+*               
+*          - DETERMINE OIL RIG ON FIRE
+*          - DETERMINE PATH
+*   
+**************************************************/
+    
+    //TODO: EXTEND ARM AND FACE OILRIGS
+    
+    //OILRIG 1 DISTANCE READING
+
+    //TODO: ROTATE ARM TO NEXT OIL RIG
+    
+    //OILRIG 2 DISTANCE READING  
+    
+    //ROTATE ARM TO NEXT OIL RIG
+    
+    //OILRIG 3 DISTANCE READING
+
+}
+
+
+
+/************
+
+Servo Functions
+
+**************/
+
+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);        
+}
+
+void initServoDriver(void) {
+    pwm.begin();
+    //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;
+    */
+    
+    setServoPulse(0, 2300);
+    setServoPulse(1, 500);
+    setServoPulse(2, 600);
+    setServoPulse(3, 2450);
+    setServoPulse(4, 2450); 
+    }
+    
+void servoPosition(int set){   
+            //moves to current position
+            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);
+            setServoPulse(4, Arm_Table[set].claw_arm); 
+            setServoPulse(5, Arm_Table[set].claw_rotate); 
+            setServoPulse(6, Arm_Table[set].claw_open); 
+}
+
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Mon Mar 03 15:27:32 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/a9913a65894f
\ No newline at end of file