File content as of revision 6:f76e93f043e2:

/*
ES200 1121 Project 2 Herndon Cover Toss - threaded version
Get threaded!
MIDN 3/c Roach, Duffey, Marasco, Zaharakis, Hersom
*/

#include "mbed.h" // note this code uses mbed OS 5
#include "rtos.h"
#include "stdio.h"
#include "Motor.h"
#include "Servo.h"


// Connections to hardware defined here
Serial pc(USBTX,USBRX); // connection to laptop for debugging
Motor launch_motor(p26, p30, p29); //launches the cover
Servo herndon_servo(p23); //servo to move monument on turn table
Servo traverse_servo(p22); //servo to adjust horizontal angle of arm
DigitalIn herndon_sw1(p17); //moves herndon monument on turn table continuously while switch is on
AnalogIn traverse_pot(p16);  //turn dial to adjust servo for horizontal angle of arm SWITCH 2
DigitalIn launch_sw3(p18); //releases the cover when switch 3 is turned on
DigitalIn recycle_sw4(p19); //retracts arm back to starting position to try again
//DigitalIn sw1 (p16) ;     not sure why this is here - maybe broken pin

// You guys are supposed to include at least a few EXTERNAL leds. 
DigitalOut heartbeat_led(p5); // indicates heartbeat
DigitalOut aiming_led(p6); // indicates aiming state
DigitalOut firing_led(p7); // indicates firing
DigitalOut recycle_led(p8); // indicates recycling 



// Herndon wiggling thread stuff
Thread herndon_thread;  // thread to run Herndon in
void herndon_callback(void);  // function for driving Herndon

// Heartbeat indication thread
Thread heartbeat_thread; // thread for heartbeat indication
void heartbeat_callback(void);  // function for heartbeat indication




/*
Thread herndon_thread runs a proces to make Herndon Monument move continuously,
as controlled by DigitalIn herndon_sw1. 

Thread heartbeat_thread just blinks a light to know code is alive and running. 

The main() thread contains a loop that contains a state machine implementing the 
following states:
1. aiming state: use Servo traverse_servo to turn the cover launch mechanism to 
match position of AnalogIn traverse_pot

2. firing state, entered when launch_sw3 is high: use launch_motor to launch cover
3. recovery state, entered when recycle_sw4 is high: return back to battery
Then goes back to aiming state.
*/
int main(void)
{
    // setup stuff
    pc.printf("ES200 1121 Project 2 Group 3\r\n");
    pc.printf("Herndon Cover Toss Game\r\n");
    pc.printf("main() thread running\r\n");
    herndon_servo.calibrate(0.0009, 90.0); // HiTec HS422 servo ES200/202 numbers
    traverse_servo.calibrate(0.0009, 90.0); // HiTec HS422 servo ES200/202 numbers
    launch_motor.speed(0.0);

    // start the Herndon thread
    herndon_thread.start(callback(herndon_callback));
    
    
    // start the heartbeat thread
    heartbeat_thread.start(callback(heartbeat_callback));
    

    // main loop for user interaction goes here
    while(1){
        
        // aiming state
        pc.printf("main() entering aiming mode, turn dial to aim cover\r\n"); 
        aiming_led.write(1); 
        while(!launch_sw3.read()){
             printf("traverse_pot.read()");
            pc.printf("main() aiming, dial at %f, servo at %f\r\n",traverse_pot.read(),traverse_servo.read()); 
            traverse_servo.write(traverse_pot.read());
           
            ThisThread::sleep_for(200); 
            }
        aiming_led.write(0); 
        
        // firing state
        firing_led.write(1); 
        pc.printf("main() thread entering cover launch sequence\r\n"); 
        pc.printf("main() thread motor launch cover\r\n");
        launch_motor.speed(-1.0);
        ThisThread::sleep_for(400);
        pc.printf("main() thread motor off\r\n"); 
        launch_motor.speed(0.0); 
        pc.printf("main() thread awaiting order to recycle\r\n"); 
        firing_led.write(0); 
        while(!recycle_sw4.read()){
        
            ThisThread::sleep_for(200);
            } // wait for user to
        
        // recovery state
        recycle_led.write(1); 
        pc.printf("main() thread recycling catapult back to battery\r\n"); 
        pc.printf("main() thread motor reversing back to battery \r\n");
        launch_motor.speed(0.25);
        ThisThread::sleep_for(600); 
        pc.printf("main() thread motor off\r\n");
        launch_motor.speed(0.0);
        ThisThread::sleep_for(1000); 
        pc.printf("main() thread back in battery\r\n"); 
        recycle_led.write(0); 
        }// while(1)
    

} // main()


    
    
    
    
    



/** Callback for running Herndon wiggling thread as its own self-contained thing
*/
void herndon_callback(void)
{
    // declare local variables
    float x=0.0; // current position of Herndon
    float xinc=0.1; // increment to move Herndon each time step

    // herndon_callback thread startup
    printf("herndon_thread running\r\n");

    // herndon_thread loop here
    while(1) {
        if (herndon_sw1.read()) {
            x = x + xinc; // move
            if (x > 1.0) {
                x = 1.0;
                xinc = -0.1; // switch to moving left
                pc.printf("herndon_thread hit right end, now moving left\r\n");
        
            } // hit the right end
            
            else if (x < 0.0) {
                x = 0.0;
                xinc = 0.1; // switch to moving right
                pc.printf("herndon_thread hit left end, now moving right\r\n");
            } // hit the left end
            
            herndon_servo.write(x);
            pc.printf("herndon_thread herndon_servo at %f\r\n",x);
        } // s1 is on

        else {
            herndon_servo.write(0.0);
            pc.printf("herndon_thread herndon_servo at %f\r\n",x);


        } // s1 if off

        ThisThread::sleep_for(200); // change this number if too fast or too slow
    } // while(1)

}// herndon_callback()


/** Callback for heartbeat indication to indicate program is alive
*/
void heartbeat_callback(void)
{
    // heartbeat_thread startup
    pc.printf("heartbeat_thread running\r\n");
    
    // heartbeat_thread loop here
    while(1) {
        heartbeat_led = !heartbeat_led;
        ThisThread::sleep_for(500);
    } // while(1)
} // heartbeat_callback()