Diff: pwm.cpp

Revision:

0:f15aa1706e16

Child:

1:b3ee94985eca

Child:

2:d7cd4deef7be

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/pwm.cpp	Mon Dec 14 15:03:32 2015 +0000
@@ -0,0 +1,90 @@
+#include "pwm.h"
+
+MotorDriver::MotorDriver() : M1(PA_6), M2(PA_7), M3(PB_6) {
+    unsigned int i;
+    M1.period_ms(20);
+    M2.period_ms(20);
+    M3.period_ms(20);
+    
+    M1.pulsewidth_us(1900);
+    M2.pulsewidth_us(600);
+    M3.pulsewidth_us(2000);
+    wait(2.0f);
+    // first move
+    for(i=0;i<1000;i++)
+    {
+        M1.pulsewidth_us(1900 - (unsigned int)(i*0.853));
+        printf("%d",1900 - (unsigned int)(i*0.853));
+    }
+    for(i=0;i<1000;i++)
+    {
+        M3.pulsewidth_us(2000 - (unsigned int)(i*0.626));
+    }
+    M2.pulsewidth_us(591);
+    wait(2.0f);
+    
+    // Look up table
+    
+    du1[0] = 1000;
+    du2[0] = 500;
+    du3[0] = 200;
+    
+    du1[3] = 10000;
+    du2[3] = 500;
+    du3[3] = 200;
+    
+    du1[4] = 10000;
+    du2[3] = 500;
+    du3[3] = 200;
+
+}
+
+void MotorDriver::Actuate() {
+    M1.pulsewidth_us(D1);
+    M2.pulsewidth_us(D2);
+    M3.pulsewidth_us(D3);
+    //printf("d1 = %d, d2 = %d, d3 = %d\r\r",D1,D2,D3);
+}
+
+// 100/9
+void MotorDriver::Angle2Duty(float a, float b, float c){
+    float k;
+    
+    k =  (float) (100.0/9.0); // degree to radian
+    D1 = (unsigned int) (k*a);
+    D2 = (unsigned int) (k*b);
+    D3 = (unsigned int) (k*c);
+}
+
+void MotorDriver::DirectDrive(unsigned int d1, unsigned int d2, unsigned int d3){
+    M1.pulsewidth_us(d1);
+    M2.pulsewidth_us(d2);
+    M3.pulsewidth_us(d3);
+    printf("PW1 = %d us,PW2 = %d us,PW3 = %d us \n\r",d1,d2,d3);
+
+}
+void MotorDriver::DriveTable(unsigned int index){
+    M1.pulsewidth_us(du1[index]);
+    M2.pulsewidth_us(du2[index]);
+    M3.pulsewidth_us(du3[index]);
+}
+
+
+unsigned int MotorDriver::GetD1 (void){
+    return D1;
+}
+unsigned int MotorDriver::GetD2 (void){
+    return D2;
+}
+unsigned int MotorDriver::GetD3 (void){
+    return D3;
+}
+unsigned int MotorDriver::Getdu1 (unsigned int i){
+    return du1[i];
+}
+unsigned int MotorDriver::Getdu2 (unsigned int i){
+    return du2[i];
+}
+unsigned int MotorDriver::Getdu3 (unsigned int i){
+    return du3[i];
+}
\ No newline at end of file