Useless Robot

Diff: main.cpp

Revision:

3:a7bd2f5946ed

Parent:

2:69a3683834c7

--- a/main.cpp	Thu Apr 27 03:53:55 2017 +0000
+++ b/main.cpp	Mon May 01 10:56:10 2017 +0000
@@ -1,175 +1,205 @@
 #include "mbed.h"
 #include "rtos.h"
-#include "Servo.h"
-#include "RGBLed.h"
-#include "wave_player.h"
-#include "SDFileSystem.h"
 #include "Motor.h"
-#include "uLCD_4DGL.h"
+
+Serial pc(USBTX, USBRX);
  
 DigitalOut led1(LED1);
 DigitalOut led2(LED2);
 DigitalOut led3(LED3);
 DigitalOut led4(LED4);
 
-// RGBLed myRGBled(p21,p22,p23); //RGB PWM pins
-
-InterruptIn Switch(p15);
+DigitalIn Switch(p15);
 
 //SERVO for arm
-Servo arm(p22);
+PwmOut arm(p26);
 
 //LEDS
-PwmOut red(p21);
-PwmOut green(p22);
+PwmOut red(p25);
+PwmOut green(p24);
 PwmOut blue(p23);
 
+DigitalOut MouthUp(p11);
+DigitalOut MouthDown(p12);
+DigitalOut MouthCenter(p10);
+
 //SD card/audio
-PwmOut PWMout(p25);
+PwmOut Speaker(p22);
+/* //For SD custom songs
 AnalogOut DACout(p18);
 SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board
-wave_player waver(&DACout,&PWMout);
+wave_player waver(&DACout,&Speaker);
 char buffer[20];
 FILE *wave_file;
-
+*/
 //motor for wheels
-Motor wheelF(p21, p6, p5); // pwm, fwd, rev
-Motor wheelB(p21, p6, p5); // pwm, fwd, rev
 
-Thread t1; // Arm Response
+Motor wheel(p21, p6, p5); // pwm, fwd, rev
+
 Thread t2; // LED Color
 Thread t3; // Speaker Music / Audio
 Thread t4; // Motor Control
 
 Mutex state;
 int angerlevel;
+int priority=1;
 bool pushFlag = false;
  
-void ArmThread() {
-    //basic if switch is toggled then use arm to turn it off
-    //TODO: setup angle/speed for reasonable opening according to angerlevel
-    //Want to add delay depedning on anger level
-    //Potentially need a queue system
-    if (pushFlag)
-    {
-        //start servo
-        while (1)
-        {
-                for(float p=0; p<1.0; p += 0.1)
-                {
-                    arm = p;
-                    wait(0.4);
-                }
-                for(float p=1; p>0.0; p -= 0.1)
-                {
-                    arm = p;
-                    wait(0.4);
-                }
-        }
-        state.lock();   //lock to update the anger level
-        angerlevel++;
-        state.unlock();
-    }
-}
 
 void LEDThread() {
     //TODO: Add colors according to angerlevel
-    state.lock();   //lock to read the angerlevel
-    switch(angerlevel){
-        case 1:
-            state.unlock();
-            //turn on leds
-            break;
-        case 2:
+    while(1){
+        if (pushFlag&&priority==1)
+        {
+            state.lock();   //lock to read the angerlevel
+            switch(angerlevel){
+                case 1:
+                    //turn on leds
+                    MouthCenter=1;
+                    MouthUp=1;
+                    red=1.0;
+                    green=1.0;
+                    blue=1.0;
+                    break;
+                case 2:
+                    //LEDS/AUDIO
+                    red=1.0;
+                    green=0.0;
+                    blue=1.0;
+                    MouthUp=0;
+                    break;
+                case 3:
+                    MouthDown=1;
+                    red=1.0;
+                    green=0.0;
+                    blue=0.0;
+                    break;
+                default:
+                    //case 0 do nothing
+                    break;
+            }
+            priority=2;
             state.unlock();
-            //LEDS/AUDIO
-            break;
-        case 3:
-            state.unlock();
-            //LEDS/AUDIO/motor
-            break;
-        default:
-            state.unlock();
-            //case 0 do nothing
-            break;
+        }
     }
-    state.unlock();
 }
 
 void AudioThread() {
     //PWM/AMP? to play audio?
     //TODO: get sound on SD card and confirm this will work
     //mabye multiple sounds according to what angerlevel
-    state.lock();   //lock to read the angerlevel
-    switch(angerlevel){
-        case 2:
+    while(1){
+        if (pushFlag&&priority==2)
+        {
+            state.lock();   //lock to read the angerlevel
+            switch(angerlevel){
+                case 2:
+                    //AUDIO
+                    /*for SD custom sound
+                    for (int a=1;a<40; a++){
+                        sprintf(buffer,"/sd//voice%d.wav",a);    
+                        wave_file=fopen(buffer,"r");
+                        waver.play(wave_file);
+                        fclose(wave_file);
+                    }*/
+                    //siren affect
+                    arm.period(1.0/800.0);
+                    Speaker = float(48)/50.0;
+                    wait(2.5);
+                    Speaker=0;
+                    break;
+                case 3:
+                    break;
+                default:
+                    //Nothing to do here
+                    break;
+            }
+            priority=3;
             state.unlock();
-            //AUDIO
-                for (int a=1;a<40; a++){
-                    sprintf(buffer,"/sd//voice%d.wav",a);    
-                    wave_file=fopen(buffer,"r");
-                    waver.play(wave_file);
-                    fclose(wave_file);
-                }
-            break;
-        case 3:
-            state.unlock();
-            //AUDIO/MOTOR
-            break;
-        default:
-            state.unlock();
-            //Nothing to do here
-            break;
+        }
     }
-    state.unlock();
 }
 
 void WheelsThread() {
     //Drive around randomly
     //TODO: add random driving for both motors(current:example for 1 motor)
-    state.lock();   //lock to read the angerlevel
-    switch(angerlevel){
-        case 3:
+    while(1){
+        if (pushFlag&&priority==3)
+        {
+
+            state.lock();   //lock to read the angerlevel
+            switch(angerlevel){
+                case 3:
+                        arm.period(0.001);
+                        wheel.speed(1.0);
+                        wait(2.5);
+
+                        arm.period(1.0/969.0);
+                        Speaker = float(46)/50.0;
+                        wait(.5);
+                        arm.period(1.0/800.0);
+                        wait(.5);
+
+                        arm.period(0.001);
+                        wheel.speed(-1.0);
+                        wait(2.5);
+                        
+                        arm.period(1.0/969.0);
+                        Speaker = float(48)/50.0;
+                        wait(.5);
+                        arm.period(1.0/800.0);
+                        wait(.5);
+                        Speaker=0;
+
+                        arm.period(0.001);
+                        wheel.speed(0.0);
+                        wait(0.04);
+                    break;
+                default:
+                    //nothing
+                    break;
+            }
+            priority=1;
             state.unlock();
-                for (float s= -1.0; s < 1.0 ; s += 0.01) {
-                   wheelF.speed(s); 
-                   wait(0.04);
-                }
-                for (float s= 1.0; s > -1.0 ; s -= 0.01) {
-                   wheelF.speed(s); 
-                   wait(0.04);
-                }
-            break;
-        default:
-            state.unlock();
-            //nothing
-            break;
+        }
     }
-    state.unlock();
 }
 
  
 void fallInterrupt(){
+    priority=1;
     pushFlag=true;
 }
 
 void init(){
-    Switch.fall(&fallInterrupt);
-    Switch.mode(PullUp); 
 
-
-    //setup PWM hardware for a Class D style audio output
-    PWMout.period(1.0/400000.0);
-
+    //start threads
+    t2.start(LEDThread);
+    t3.start(AudioThread);
+    t4.start(WheelsThread);
 
 }
 
 int main() {
     init();
-        
-    t1.start(ArmThread);
-    t2.start(LEDThread);
-    t3.start(AudioThread);
-    t4.start(WheelsThread);
-    
+
+    while(1) {
+        while(Switch) {
+            //* code 
+        }
+        fallInterrupt();
+            //start servo
+            state.lock();   //lock for period + angerlevel
+            arm.period(0.020);          // servo requires a 20ms period
+            arm.pulsewidth(0.002);
+            wait(0.5);
+            arm.pulsewidth(0.0013);
+            wait(0.5);
+
+            angerlevel++;
+            state.unlock();
+            wait(1);
+            pushFlag=false;
+    }
+    pc.printf("exited");
 }
\ No newline at end of file