Revision 6:2fa349da33bb, committed 2016-12-05

Comitter:

apcastelein

Date:

Mon Dec 05 23:37:02 2016 +0000

Parent:

5:507618748cb4

Commit message:

Added Code for arms

Changed in this revision

diff -r 507618748cb4 -r 2fa349da33bb main.cpp
--- a/main.cpp	Mon Dec 05 22:45:09 2016 +0000
+++ b/main.cpp	Mon Dec 05 23:37:02 2016 +0000
@@ -8,9 +8,13 @@
 //   https://developer.mbed.org/components/LSM9DS1-IMU/
 LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);
 float initial_direction;
-Motor left(p21, p22, p23, 1);
+
+//wheel motors
+Motor left(p21, p20, p23, 1);
 Motor right(p26, p25, p24, 1);
 
+//arm motor
+Motor arm(p22, p7, p6,1);
 
 //Color sensor, used to assist robot positioning
 // - wired using i2c according to
@@ -25,8 +29,8 @@
 Serial pc(USBTX, USBRX);
 
 //initialize and calibrate IMU
-/*
-void initIMU(){
+
+/*void initIMU(){
     IMU.begin();
     if (!IMU.begin()) {
         pc.printf("Failed to communicate with LSM9DS1.\n");
@@ -34,9 +38,9 @@
     IMU.calibrate(1);
     IMU.calibrateMag(0);   
     
-   // initial_direction = getDirection();
-}
-*/
+   initial_direction = getDirection();
+}*/
+
 //verify color sensor and initialize
 void initColorSensor(){
     pc.baud(9600);
@@ -61,10 +65,17 @@
     color_sensor.write(color_addr,enable_register,2,false);   
 }
 
+void loadArm(){
+    arm.speed(-.3);
+    wait(2.7);
+    arm.speed(0);        
+}
+
 //initialization for all components
 void init(){
     //initIMU();    
     initColorSensor();
+    loadArm();
 }
 
 // Sends a request to the server and waits for a path response
@@ -129,8 +140,16 @@
 void rotateRight(){}
 
 //arem technology is yet to be determined, will probably involve adjusting a motor
-void armUp(){}
-void armDown(){}
+void armUp(){
+    arm.speed(.3);
+    wait(2.5);
+    arm.speed(0);            
+}
+void armDown(){
+    arm.speed(-.3);
+    wait(2.5);
+    arm.speed(0);                
+}
 
 // Loops through path commands and executes each one sequentially
 void executePath(char* path){
@@ -150,15 +169,21 @@
 //- repeatedly listens for paths and then executes them
 int main() {
     init();
+    //rotateLeft();
+    wait(5);
     while(true){
-        while(!colorEntered()) wait(.1);
+        armUp();
+        wait(3);
+        armDown();
+        wait(3);
+        /*while(!colorEntered()) wait(.1);
         green = 1;
         wait(5);
         green = 0;
         rotateLeft();
         left.speed(0);
         right.speed(0);
-        wait(1);
+        wait(1);*/
         /*green = colorEntered();
         pc.printf("%f\r\n",getDirection());
         pc.printf("%f\r\n",getRelativeDirection());