main.cpp

00001 #include "mbed.h"
00002 #include "arm.h"
00003 
00004 Arm arm(p9,p10);
00005 //Motor 1
00006 DigitalIn button1open(p20);
00007 DigitalIn button1close(p21);
00008 //Motor 2
00009 DigitalIn button2left(p19);
00010 DigitalIn button2right(p22);
00011 //Motor 3
00012 DigitalIn button3up(p18);
00013 DigitalIn button3down(p23);
00014 //Motor 4
00015 DigitalIn button4up(p17);
00016 DigitalIn button4down(p24);
00017 //Motor 5
00018 DigitalIn button5up(p16);
00019 DigitalIn button5down(p25);
00020 //Motor 6
00021 DigitalIn button6left(p15);
00022 DigitalIn button6right(p26);
00023 
00024 InterruptIn throwButton(p27);
00025 
00026 void initialPos();
00027 void throwMode();
00028 void move(int16_t motorNumber, bool switch1, bool switch2, 
00029             int16_t * motor, 
00030             int16_t max_pos, int16_t min_pos, int16_t mov);
00031 
00032 int16_t motor2=125, motor3=500, motor4=500, motor5=500, motor6=500;
00033 
00034 int main(){
00035     throwButton.rise(&throwMode);
00036     initialPos();
00037     
00038     while(1){
00039         
00040         if(button1open)
00041             arm.moveServo(1,500,700);
00042         else if(button1close)
00043             arm.moveServo(1,0,700);
00044         
00045         //motor 2       
00046         move(2,button2left,button2right,
00047                 &motor2, 
00048                 1000, 0, 25);
00049         
00050         //motor 3        
00051         move(3,button3up,button3down,
00052                 &motor3, 
00053                 1000, 0, 25);
00054             
00055         //motor 4        
00056         move(4, button4down, button4up,
00057                 &motor4, 
00058                 1000, 0, 25);
00059         
00060         //motor 5        
00061         move(5,button5up,button5down,
00062                 &motor5, 
00063                 1000, 0, 25);
00064         //motor 6
00065         move(6,button6left,button6right,
00066                 &motor6, 
00067                 1000, 200, 25);
00068                 
00069         wait(0.1);
00070     }       
00071     
00072 }
00073 
00074 void move(int16_t motorNumber, bool switch1, bool switch2, 
00075             int16_t * motor, 
00076             int16_t max_pos, int16_t min_pos, int16_t mov){
00077                 
00078     int16_t prev = *motor;
00079     if(switch1)
00080             *motor = ((*motor-mov) >= min_pos) ? *motor-mov : min_pos;
00081     else if(switch2)
00082         *motor = ((*motor+mov) <= max_pos) ? *motor+mov : max_pos;
00083     if (prev!=*motor)
00084             arm.moveServo(motorNumber,*motor,100);
00085 }
00086 
00087 void throwMode(){
00088     
00089     bool active = true;
00090     
00091     //Throw possition
00092     arm.moveServo(2,125,500);
00093     arm.moveServo(3,700,1000);
00094     arm.moveServo(4,350,1000);
00095     arm.moveServo(5,700,1000);
00096     arm.moveServo(6,500,1000);
00097     
00098     while(active){
00099         if(button1open)
00100             arm.moveServo(1,500,700);
00101         else if(button1close)
00102             arm.moveServo(1,0,700);
00103         
00104         if(button2left || button2right){
00105             motor2=125, motor3=450, motor4=600, motor5=500, motor6=500;
00106             
00107             //Throw function
00108             arm.moveServo(3,motor3,300);
00109             arm.moveServo(4,motor4,300);
00110             arm.moveServo(5,motor6,300);
00111             wait(0.13);
00112             arm.moveServo(1,0,200);
00113             
00114             active=false;
00115             
00116         } 
00117     }
00118 }
00119 
00120 void initialPos(){
00121     arm.moveServo(1,500,1000);
00122     arm.moveServo(2,125,500);
00123     arm.moveServo(3,500,1000);
00124     arm.moveServo(4,500,1000);
00125     arm.moveServo(5,500,1000);
00126     arm.moveServo(6,500,1000);
00127 }