Revision 33:2a96d2e00b46, committed 2013-11-18

Comitter:

Sparker

Date:

Mon Nov 18 06:23:55 2013 +0000

Parent:

10:e52a6e1bbb48

Commit message:

slave mbed

Changed in this revision

--- a/Master.cpp	Tue Nov 12 20:10:03 2013 +0000
+++ b/Master.cpp	Mon Nov 18 06:23:55 2013 +0000
@@ -0,0 +1,16 @@
+// Reply to a SPI master as slave
+ 
+ #include "mbed.h"
+ 
+ SPISlave device(p5, p6, p7, p8); // mosi, miso, sclk, ssel
+ 
+ int main() {
+     device.reply(0x00);              // Prime SPI with first reply
+     while(1) {
+         if(device.receive()) {
+             int v = device.read();   // Read byte from master
+             v = (v + 1) % 0x100;     // Add one to it, modulo 256
+             device.reply(v);         // Make this the next reply
+         }
+     }
+ }
\ No newline at end of file

--- a/MotorSlave.cpp	Tue Nov 12 20:10:03 2013 +0000
+++ b/MotorSlave.cpp	Mon Nov 18 06:23:55 2013 +0000
@@ -1,21 +1,25 @@
 #include "mbed.h"       // mbed library
 #include "QEI.h"        // quadrature encoder library to count encouder ticks
-#include "PID.hpp"
+
 
 // Declaring pins
-// DigitalOut dOut(p5);    // pin 5 set as a digital output.  Pins 5-30 can be used as digital outputs.
-// DigitalIn dIn(p6);      // pin 6 set as digital input.  Pins 5-30 can be used as digital inputs.
-DigitalOut Forward(p18);
-DigitalOut Backward(p19);
-AnalogIn aIn(p15);      // pin 15 set as analog input.  Pins 15-20 can be used as analog inputs.
-PwmOut pwmOut(p21);     // pin 21 set as PWM output.  Pins 21-26 can be used as PWM outputs.
+DigitalOut Forward1(p17);
+DigitalOut Backward1(p18);
+AnalogIn aIn1(p15);      // pin 15 set as analog input.  Pins 15-20 can be used as analog inputs.
+PwmOut pwmOut1(p21);     // pin 21 set as PWM output.  Pins 21-26 can be used as PWM outputs.
+
+DigitalOut Forward2(p19);
+DigitalOut Backward2(p20);
+AnalogIn aIn2(p16);      // pin 15 set as analog input.  Pins 15-20 can be used as analog inputs.
+PwmOut pwmOut2(p22);     // pin 21 set as PWM output.  Pins 21-26 can be used as PWM outputs.
 
 // Declare other objects
-Ticker myTicker;                // creates an instance of the ticker class, which can be used for running functions at a specified frequency.
-Serial mySerial(USBTX, USBRX);  // create a serial connection to the computer over the tx/rx pins
-LocalFileSystem local("local");               // Create the local filesystem under the name "local"
-QEI myEncoder(p25, p26, NC, 1200, QEI::X4_ENCODING); // (encoder channel 1 pin, encoder channel 2 pin, index (n/a here), counts per revolution, mode (X4, X2))
-// Use line above verbatim to avoid difficulties. Only certain pins can be used for counting encoder ticks.
+Ticker T1;                // creates an instance of the ticker class, which can be used for running functions at a specified frequency.
+Ticker T2;                // creates an instance of the ticker class, which can be used for running functions at a specified frequency.
+QEI Encoder1(p25, p26, NC, 1200, QEI::X4_ENCODING); // (encoder channel 1 pin, encoder channel 2 pin, index (n/a here), counts per revolution, mode (X4, X2))
+QEI Encoder2(p23, p24, NC, 1200, QEI::X4_ENCODING); // (encoder channel 1 pin, encoder channel 2 pin, index (n/a here), counts per revolution, mode (X4, X2))
+
+SPISlave device(p5, p6, p7, p8); // mosi, miso, sclk, ssel
 
 // Declare global variables
 int x = 1200;           // an int is a 32-bit integer (-2147483648 to 2147483647)
@@ -25,28 +29,115 @@
 float ki = 0;
 float cur_des = 0.2;
 
+float speed = 0;
+float pos = 0;
+float timet = 0;
+float dCurrent1 = 0;
+float dCurrent2 = 0;
+float duty1 = 0;
+float duty2 = 0;
+float mCurrent1 = 0;
+float mCurrent2 = 0;
+float dAngle1=0; // Desired Angle
+float dAngle2=0;
+float dAngularVelocity1=0; // Desired Angular velocity
+float dAngularVelocity2=0;
+float Angle1 = 0;
+float Angle2 = 0;
 
-float getMotorPos()
-{
-    return myEncoder.getPulses();
+float filterLowPass(float old, float current, float alpha){
+    return (old+alpha*(current-old));
 }
 
-float getCurrent()
+//void Printer(){
+//fprintf(fp, "%f    %f    %f\n", duty1, mCurrent1, speed);
+//}
+
+float getMotorPos(int a)
 {
-    float curr = (1/.140)*3.3 * aIn;
-    // float curr_err = cur_des - curr;
-    mySerial.printf("Current: %F   \n\r", curr);
+    if (a == 1){
+    return Encoder1.getPulses();}
+    else{
+    return Encoder2.getPulses();}
+}
+
+float getCurrent(int a)
+{
+    float curr = 0;
+    if (a == 1) {
+     curr = (1/.140)*3.3 * aIn1;
+    }
+    else {
+     curr = (1/.140)*3.3 * aIn2;
+    }
     return curr;
 }
 
-void setPWM(float pwmDes)
+void Control()
 {
-    pwmOut.write(pwmDes);
+float preAngle=Angle1; // Storing the angle in the previous time step
+Angle1 = Encoder1.getPulses()/1200.0*6.283; // Updating new motor angle
+speed =filterLowPass(speed,(Angle1-preAngle)/.001, .2); // Updating new motor angular vel
+mCurrent1 = getCurrent(1);
+
+preAngle=Angle2; // Storing the angle in the previous time step
+Angle2 = Encoder2.getPulses()/1200.0*6.283; // Updating new motor angle
+speed =filterLowPass(speed,(Angle2-preAngle)/.001, .2);
+mCurrent2 = getCurrent(2);
+
+dCurrent1 = (3*(dAngle1-Angle1)+0.04*(dAngularVelocity1-speed));
+dCurrent2 = (3*(dAngle2-Angle2)+0.04*(dAngularVelocity2-speed));
+
+
+if (dCurrent1>0){
+Forward1=1;
+Backward1=0;
+duty1= (abs(dCurrent1)*3.27+0.174*speed+10*(abs(dCurrent1)-mCurrent1))/12.0; //2.10- Resistance 0.2-Kv 5- current control gain, 12- source voltage
+} 
+else if (dCurrent1<0) {
+Forward1=0;
+Backward1=1;
+duty1= (abs(dCurrent1)*3.27-0.174*speed+10*(abs(dCurrent1)-mCurrent1))/12.0;}
+
+if (duty1>0.99){
+duty1 =1;}
+
+if (dCurrent2>0){
+Forward2=1;
+Backward2=0;
+duty1= (abs(dCurrent2)*3.27+0.174*speed+10*(abs(dCurrent2)-mCurrent2))/12.0; //2.10- Resistance 0.2-Kv 5- current control gain, 12- source voltage
+} 
+else if (dCurrent2<0) {
+Forward2=0;
+Backward2=1;
+duty1= (abs(dCurrent2)*3.27-0.174*speed+10*(abs(dCurrent2)-mCurrent2))/12.0;}
+
+if (duty2>0.99){
+duty2 =1;}
+
+pwmOut1.write(duty1);
+pwmOut2.write(duty2);
 }
   
 // the main function runs once
-int main() 
-{
-    pwmOut.period(.0005);          // Set PWM output freqency to ________
+int main() {
+    dCurrent1 = 3;
 
+//    fp = fopen("/local/out.txt", "w");  // Open "out.txt" on the local file system for writing
+//    T2.attach(&Printer, .005);
+    T1.attach(&Control, .001);
+    
+    pwmOut1.period_us(.0005);
+    pwmOut2.period_us(.0005);
+        
+    //pwmOut.write(.5);
+    wait(150);
+    dCurrent1 = 0;
+    T1.detach();
+    T2.detach();
+//    fclose(fp);
+
+    pwmOut1.write(0);
+    pwmOut2.write(0);
+        
 }
\ No newline at end of file