File content as of revision 22:c18f04d1dc49:

#include "QEI.h"
#include "Motor.h"
#include "pot.h"
#include "list.h"
#include <time.h>
#include "pid.h"
 
Serial pc(USBTX, USBRX);
//Use X4 encoding.
//QEI wheel(p29, p30, NC, 624, QEI::X4_ENCODING);
//Use X2 encoding by default.
QEI encoder (PTC16, PTC17, PTB9, 512);
Motor motor(PTC4, PTD0, PTC3, PTC12, PTB23, 2000);
Pot pend(PTC10, &encoder,0);
PID pid = PID(pend.UPDATE_TIME, 4700, -4700, 500, 0, 0); 
Ticker update;

void test_distance(){
    int wait = 1000;
    int steps = 100;
    while (wait > 500){
        while (steps > 10){
            printf("steps: %i wait: %i \r\n", steps, wait);
            for( int i = 0; i < 3; i++){
                encoder.reset();
                motor.step_clockwise(steps, wait);
                int pulses_cw = encoder.getPulses();
                wait_ms(200);
                int coast_pulses_cw = encoder.getPulses();
                
                encoder.reset();
                motor.step_anticlockwise(steps, wait);
                int pulses_ccw = encoder.getPulses();
                wait_ms(200);
                int coast_pulses_ccw = encoder.getPulses();
                printf("trial: %i \tclockwise pluses: \t%i  \tanticlockwise pluses: \t%i \r\n", i + 1, pulses_cw, pulses_ccw);
                printf("\t\tpulses after coast: \t%i \tpulses after coast: \t%i\r\n", coast_pulses_cw, coast_pulses_ccw);
            }
            steps -=10;
            printf("\r\n");
        }
        
        wait-=100;
        steps=100;
    }       
}



void calibrate_pendulum(){
    encoder.reset();
    while(1){
        printf("voltage %%: %f  pulse: %i\r\n", pend.angle_as_voltage(), encoder.getPulses());
    }
}
void move_pulses(int pulses, int wait){ // find number of pulses from the encoder going from one end to the other.
    int start = encoder.getPulses();
    if(pulses >= 0){
        while(encoder.getPulses() < start + pulses){
            motor.clockwise(wait);    
        } 
    }else{
        while(encoder.getPulses() > start + pulses){
            motor.anticlockwise(wait);
        }
    } 
}
void test_speed(){
    int speed = 1000;
    int decrement = 10;
 
    List velocities_cw = List(1000);
    List velocities_ccw = List(1000);
    printf("hi");
    while(speed > 400){
        for(int i =0; i<100; i++){
            motor.clockwise(speed);
            velocities_cw.add(pend.velocity);
        }
        wait(1);
        for(int i =0; i<100; i++){
            motor.anticlockwise(speed);
            velocities_ccw.add(pend.velocity);
        }
        wait(1);
        printf("clockwise velocity peak: %f avg: %f anticlockwise velocity peak: %f avg: %f \r\n"
         ,velocities_cw.min(), velocities_cw.average(),  velocities_ccw.max(), velocities_ccw.average());
        speed -= decrement;
    }       
}

void update_handler(){
    pend.update();
    double output = pid.calculate(180, pend.angle);
    //printf("output: %f angle: %f\r\n",output , pend.angle);
    if(output > 0){ 
        motor.wait = 5000 - (int) output;
        motor.dir = false;
    }else{
        motor.wait = 5000 + (int) output;
        motor.dir = true;
    }  
}
int main(){
    wait(2);
    pend.set_zeros();
    update.attach(&update_handler, pend.UPDATE_TIME);
    while(1){
        //pend.print_test();
        motor.run(true);
        //calibrate_pendulum();
    }
    
    
    
  
    
   /* while(1){
        
        //printf("%f\r\n",pend.angle_as_voltage());
        //pend.print_test();
        if(pend.angle>2) motor.anticlockwise(1000);
        else if( (pend.angle>=-2) && (pend.angle<=2))wait_ms(1);
        else motor.clockwise(1000);
    }

   pend.set_zeros();
    int waits = 700;
    while(1){
        move_pulses(1000*neg, waits);
        neg*= -1;
        if(neg == -1)
            waits -= 10;
        printf("Wait time: %i\r\n", waits);
        pend.print_test();
        wait(3);
    }
}
  update.attach(&update_handler, pend.UPDATE_TIME);
  wait(1);
  pend.set_zeros(); 
    while(1)pend.print_test();
 */   
}