File content as of revision 23:5238b046119b:

#include "QEI.h"
#include "Motor.h"
#include "pot.h"
#include "list.h"
#include <time.h>
#include "pid.h"

#define TRACK_LENGTH 296
#define TRACK_MIDDLE TRACK_LENGTH/2
#define Kp 1
#define Ki 100
#define Kd 100
 
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, 400);
Pot pend(PTC10, &encoder,0);
PID pid = PID(pend.UPDATE_TIME, 148, -148, Kp, 0, 0); 
Ticker update;


/*
void update_handler(){
    pend.update();
//    pid.testError((float) 180, (float) pend.angle);
    float output = pid.calculate(180, pend.angle);
    //printf("output: %f angle: %f\r\n",output , pend.angle);
    if(output > 0){
        motor.delay = 3000 - (int) output;
        if (motor.dir != false) { 
            wait_us(100);
        }
        motor.dir = false;
    }else{
        motor.delay = 3000 + (int) output;
        if (motor.dir != true) {
            wait_us(100);    
        }
        motor.dir = true;
    }  
}
*/

void update_handler2(){
    pend.update();
    float output = pid.calculate(180, pend.angle);
    motor.length = int(5*output);
    if(output > 0) {
        if(motor.dir != false) {
            wait_us(100);
        }
        motor.dir = false;
    }
    else {
        if(motor.dir != true) {
            wait_us(100);  
        }
        motor.dir = true; 
    }
}


int main(){
    wait(2);
    pend.set_zeros();
    motor.initialize(TRACK_MIDDLE);
    wait(2);
    update.attach(&update_handler2, pend.UPDATE_TIME);
    while(motor.steps > 6 && motor.steps < 290){
        //pend.print_test();
        motor.run2(true);
    } 
    
/*    motor.step_clockwise(74);  
    for (int i = 1000; i != 400; i -= 50) {
        wait_us(100);
        motor.step_clockwise(148, i);
        wait_us(100);
        motor.step_anticlockwise(148, i);   
    }   
    
    while (1) {
        motor.step_clockwise(149);
        wait_us(500);
        motor.step_anticlockwise(149);
        wait_us(500);
    }        
*/
   
/* 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);
    }
}

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 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;
    }       
}

 */   
}