abrayan

Dependencies:   mbed

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers mover.cpp Source File

mover.cpp

00001 
00002 #include "mover.h"
00003 #include "mbed.h"
00004 #include "math.h"
00005 
00006 
00007 PwmOut myServo1(PA_10);
00008 PwmOut myServo2(PB_3);
00009 PwmOut myServo3(PB_5);
00010 PwmOut myServo4(PB_4);
00011 PwmOut myServo5(PB_10);
00012 PwmOut myServo6(PA_8);
00013 PwmOut myServo7(PB_6);
00014 PwmOut myServo8(PA_7);
00015 
00016 
00017 
00018 uint8_t  ss_time=180;     // tiempo  de espera para moverse 1 mm en microsegundos
00019 
00020 void put_sstime(uint8_t vtime){
00021     ss_time=vtime;
00022     
00023 }
00024 
00025 int coord2us(float coord)
00026 {
00027     if(0 <= coord <= MAXPOS)
00028         return int(750+coord*1900/180);// u6
00029     return 750;
00030 
00031 }
00032 
00033 
00034  
00035 
00036 
00037 void mover_ser(uint8_t motor, uint8_t pos){
00038 
00039     int pulseX = coord2us(pos);
00040     
00041     switch (motor)
00042     {
00043         case 0x01:  myServo1.pulsewidth_us(pulseX);
00044                     break;
00045                     
00046         case 0x02:  myServo2.pulsewidth_us(pulseX);
00047                     break;            
00048     
00049         case 0x03:  myServo3.pulsewidth_us(pulseX);
00050                     break;
00051                     
00052         case 0x04:  myServo4.pulsewidth_us(pulseX);
00053                     break;
00054                     
00055         case 0x05:  myServo5.pulsewidth_us(pulseX);
00056                     break;
00057                     
00058         case 0x06:  myServo6.pulsewidth_us(pulseX);
00059                     break;                    
00060                     
00061         case 0x07:  myServo7.pulsewidth_us(pulseX);
00062                     break;
00063                     
00064         case 0x08:  myServo8.pulsewidth_us(pulseX);
00065                     break;                    
00066     }
00067 }
00068 
00069 void init_servo()
00070 {
00071     myServo1.period_ms(20);
00072     myServo2.period_ms(20);
00073     myServo3.period_ms(20);
00074     myServo4.period_ms(20);
00075     myServo5.period_ms(20);
00076     myServo6.period_ms(20);
00077     myServo7.period_ms(20);
00078     myServo8.period_ms(20); 
00079 }