main.cpp

00001 #include "mbed.h"
00002 #include "HIDScope.h"
00003 #include "encoder.h"
00004 
00005 Encoder motor1(D13,D12);
00006 PwmOut led(D9);
00007 HIDScope scope(3);
00008 
00009 
00010 
00011 int main()
00012 {
00013     const float degrees_per_count = 90/200.0; //quick approach
00014     const float ain_degrees_range = 90; //degrees over which the potmeter can control.
00015     const float kp_1_range = -1;
00016     AnalogIn setpoint_1(A0);
00017     AnalogIn p_value(A1);
00018     DigitalOut direction_1(D4);
00019     PwmOut pwm_1(D5);
00020     float degrees_setpoint;
00021     float error;
00022     float output;
00023     pwm_1.period(1.0/10000); //10kHz
00024         
00025     while (true) {
00026         degrees_setpoint = setpoint_1 * ain_degrees_range;
00027         error = degrees_setpoint-(motor1.getPosition()*degrees_per_count); //error =  setpoint - current
00028         output = error*p_value*kp_1_range;
00029         if(output > 0)
00030         {
00031             direction_1.write(true);
00032         }
00033         else
00034         {
00035             direction_1.write(false);
00036         }
00037         pwm_1.write(fabs(output));
00038         scope.set(0,motor1.getPosition());
00039         scope.set(1,error);
00040         scope.set(2,fabs(output));
00041         led.write(motor1.getPosition()/100.0);
00042         scope.send();
00043         wait(0.01);
00044     }
00045 }