CarPWM.cpp

00001 #include "CarPWM.h"
00002 #include "mbed.h"
00003 
00004 #define MINIMUM_MOTOR_WIDTH 1160 // microseconds
00005 #define ZERO_MOTOR_WIDTH 1505    // microseconds
00006 #define MAXIMUM_MOTOR_WIDTH 1910 // microseconds
00007 #define MINIMUM_SERVO_WIDTH 1016 // microseconds
00008 #define ZERO_SERVO_WIDTH 1470    // microseconds
00009 #define MAXIMUM_SERVO_WIDTH 1858 // microseconds
00010 #define PERIOD 13500             // miliseconds
00011 #define MOTOR_RANGE_MAX 100
00012 #define SERVO_ANGLE_MAX 100
00013 
00014 void initialize(PwmOut motor, PwmOut servo){
00015     motor.period_us(PERIOD);
00016     servo.period_us(PERIOD);
00017     }
00018 void setServoPWM(float angle, PwmOut servo){
00019     if(angle > SERVO_ANGLE_MAX)
00020         angle = SERVO_ANGLE_MAX;
00021     else if(angle < -SERVO_ANGLE_MAX)
00022         angle = - SERVO_ANGLE_MAX;
00023     float servo_width = ZERO_SERVO_WIDTH + (angle/SERVO_ANGLE_MAX)*(MAXIMUM_SERVO_WIDTH - ZERO_SERVO_WIDTH); 
00024     servo.pulsewidth_us(servo_width);
00025    // Serial pc(USBTX, USBRX);
00026    // pc.printf("\nServo: %f ",servo_width);
00027 }
00028 void setMotorPWM(float speed, PwmOut motor){
00029     if(speed > MOTOR_RANGE_MAX)
00030         speed = MOTOR_RANGE_MAX;
00031     else if(speed < -MOTOR_RANGE_MAX)
00032         speed = - MOTOR_RANGE_MAX;
00033     float motor_speed = ZERO_MOTOR_WIDTH + (speed/MOTOR_RANGE_MAX)*(MAXIMUM_MOTOR_WIDTH - ZERO_MOTOR_WIDTH); 
00034     motor.pulsewidth_us(motor_speed);
00035   //  Serial pc(USBTX, USBRX);
00036   //  pc.printf("Motor: %f ",motor_speed);
00037 
00038 }