Diff: main.cpp

Revision:

0:8f37781c0054

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Apr 17 21:13:44 2015 +0000
@@ -0,0 +1,189 @@
+#include "mbed.h"
+#include "L3GD20.h"
+#include "LSM303DLHC.h"
+#include "Servo.h"
+#include "math.h"
+#include "PwmIn.h"
+
+#include "iSerial.h"
+#include "iostream"
+#include "stdio.h"
+#include "string.h"
+
+#include <string>
+#include <sstream>
+#define TS 10                    // sample time [ms]
+#define SIMULATION_TIME 10        // simulation time [s]
+
+
+
+//serial 
+Serial BB(p13, p14);  // tx, rx
+Serial pc(USBTX,USBRX);
+
+//period_pwm setup as 20ms
+//speed: 0 is off, and 1 is full speed；for servo 0-0.2 duty circle
+//direction: 0 clockwise, 1 counter-clockwise
+
+float period_pwm_ac; // Actuator current
+float period_pwm;
+
+bool directionA;
+bool directionB;
+bool directionC;
+
+float speedA;
+float speedB;
+float speedC;
+
+int flag;
+int x,y; // Image coordinate
+
+//funtion declaration
+void move(int motor, float speed, int direction);
+PwmOut PWMA(p25);//LEFT  FIN
+PwmOut PWMB(p23);//RIGHT FIN
+PwmOut PWMC(p24);//Left servo
+PwmOut PWMD(p26);//Right servo
+PwmOut PWME(p21);//Tail
+
+DigitalOut STBY (p30);
+//DigitalOut STBY2 (p29);
+DigitalOut AIN1 (p8);
+DigitalOut AIN2 (p11); // Fin1 left
+DigitalOut BIN1 (p7);  
+DigitalOut BIN2 (p6);  // Fin2 right
+DigitalOut myLed (LED1);
+DigitalOut myLed1(LED2);
+
+
+ 
+
+void moveA()
+{
+    STBY=1; //disable standby
+    int inPin1=1;
+    int inPin2=0;
+    if(directionA==0) {
+        inPin1 = 0;
+        inPin2 = 1;
+    }
+    AIN1=inPin1;
+    AIN2=inPin2;
+    directionA=!directionA;
+
+    //pc.printf("dirA = %d\n\r",directionA);
+
+
+
+    PWMA.pulsewidth(period_pwm*speedA);
+
+}
+
+void moveB()
+{
+    STBY=1; //disable standby
+    int inPin1=0;
+    int inPin2=1;
+    if(directionB==0) {
+        inPin1 = 1;
+        inPin2 = 0;
+    }
+    //pc.printf("dirB = %d\n\r",directionB);
+    BIN1=inPin1;
+    BIN2=inPin2;
+    directionB=!directionB;
+
+    PWMB.pulsewidth(period_pwm*speedB);
+
+}
+
+void stop()
+{
+//enable standby
+    STBY=0;;
+}
+
+int main()
+{
+    //serial to BBB setup    
+    char str[9];
+    char *token;
+ 
+    pc.baud(9600); 
+    BB.baud(9600); 
+    
+    //Actuator & servo setup
+    
+        period_pwm_ac=0.0020;                   //500hz
+        period_pwm=0.020;                      //20ms
+        PWMA.period(period_pwm_ac);          
+        PWMB.period(period_pwm_ac);          
+        PWMC.period(period_pwm);                // servo requires a 20ms period
+        PWMD.period(period_pwm);                // servo requires a 20ms period
+        PWME.period(period_pwm);                // servo requires a 20ms period
+        
+    //initial direction & current    
+        directionA=1;
+        directionB=0;
+        flag=1; 
+        speedA=0.2;
+        speedB=0.2; //Actuator speed control
+        
+        while(1)
+{
+            
+    //Talk to BBB
+         int i;
+         if(BB.readable()>0) 
+         {
+            for(i=0;i<9;i++) 
+            str[i] = BB.getc();
+           
+            if((0x30<str[1]<35)&&(str[8]==0x0D)&&(str[3]==0x2C))
+            {
+                token = strtok(str, ",");
+                x = atoi(token);                //100-420(width_320);No target:555 
+    
+                token = strtok(NULL, ",");
+                y = atoi(token);                //100-340(height_240); No target:555 
+               // pc.printf(" x=%d y=%d", x,y);
+              //pc.printf(str);
+            }
+         }
+         
+    //Serial test
+    if(x>210)
+    moveA();
+    else if (x<210)
+    moveB();
+    else if (x==555)
+    {
+         moveA();
+         moveB();
+         wait(0.2);
+    }
+
+
+//Actuator test
+//       moveA();
+//       moveB();
+//       wait(0.2);
+
+//Servo test
+         
+ //           PWMC.pulsewidth(period_pwm*0.08);
+ //           PWMD.pulsewidth(period_pwm*0.08);
+ //           PWME.pulsewidth(period_pwm*0.08);
+ //            wait(0.05);
+ //           PWMC.pulsewidth(period_pwm*0.05);
+ //           PWMD.pulsewidth(period_pwm*0.05);
+  //          PWME.pulsewidth(period_pwm*0.08);
+ //          
+ 
+
+        
+
+}
+            
+}
\ No newline at end of file