invertedRobot

main.cpp

/* "A very easy and simple inverted pendulum balancing robot"
 You need only half a day to make it, if you have some Materials.
  Copyright (C) 2014 ArduinoDeXXX All Rights Reserved. */ 
//modified by robo8080
#include "mbed.h"
Ticker tick;
//Timer t;

DigitalOut fwdL(PTD4);  //2
DigitalOut revL(PTA12);  //3
PwmOut pwmL(PTA4);      //4
DigitalOut fwdR(PTA5);  //5
DigitalOut revR(PTC8);  //6
PwmOut pwmR(PTC9);      //7
DigitalIn button(PTA13); //8
//AnalogIn ain(PTC2);     //A4
AnalogIn ain(PTB0);     //A0
DigitalOut ledr(LED_RED);
DigitalOut ledg(LED_GREEN);
DigitalOut ledb(LED_BLUE);
/*
DigitalOut fwdL(p15);
DigitalOut fwdR(p16);
DigitalOut revL(p17);
DigitalOut revR(p18);
PwmOut pwmL(p21);
PwmOut pwmR(p22);
DigitalIn button(p19);
AnalogIn ain(p14);
*/
DigitalOut myled(LED1);
//Serial pc1(USBTX, USBRX);
Serial pc(PTA2, PTA1);

volatile int i = 0; //02
volatile int countS = 0; //03
long zeroOmegaI = 0;// 04
volatile int recOmegaI[10]; //05
volatile int omegaI = 0; //06
volatile long thetaI = 0; //07
volatile long sumPower = 0; //08
volatile long sumSumP = 0; //09
//const int kAngle = 45; //10
const int kAngle = 49; //10
//const int kAngle = 26; //10
//const int kOmega = 85; //11
const int kOmega = 89; //11
//const int kOmega =9; //11
const long kSpeed = 57; //12
//const long kSpeed = 30; //12
//const long kDistance = 60; //13
const long kDistance = 63; //13
//const long kDistance = 78; //13
volatile long powerScale; //14
volatile int power; //15
volatile long vE5 = 0; //16
volatile long xE5 = 0; //17
//const int buttonPin = 2;
//const int ledPin =  13;
int buttonState = 0;

int flag = 0;

volatile int drct = 0;
volatile bool right = false;
volatile bool left = false;
volatile int fwdBck = 0;

int timer1 = 0;

void setup();
void loop();
void training();
void chkAndCtl();

int max(int in1,int in2)
{
    if(in1 > in2) return in1;
    return in2;
}

int min(int in1,int in2)
{
    if(in1 < in2) return in1;
    return in2;
}

int main() {
    ledr = 1;
    ledg = 1;
    ledb = 1;
    setup();
    while(1) {
        loop();
    }
}

void setup () { //18
  pc.baud(115200); //19
/*
  pinMode(ledPin, OUTPUT);
  pinMode(4, OUTPUT); //20
  pinMode(5, OUTPUT);
  pinMode(6, OUTPUT);
  pinMode(7, OUTPUT);
  pinMode(8, OUTPUT);
  pinMode(9, OUTPUT);
  
  analogWrite( 6, 0 );
  digitalWrite( 4, HIGH );
  digitalWrite( 5, LOW );
  analogWrite( 9, 0 );
  digitalWrite( 7, HIGH );
  digitalWrite( 8, LOW );
*/  

    pwmL.period(0.001);
    pwmL = 0.0;
    fwdL = 0;
    revL = 0;
    pwmR.period(0.001);
    pwmR = 0.0;
    fwdR = 0;
    revR = 0;
  
  for(i=0;i < 15; i++)
  {
    myled = 1;
    wait_ms (100);
    myled = 0;
    wait_ms (100);
  }
    myled = 1;
  
  for ( i = 0 ; i < 10 ; i++ ) { recOmegaI[i] = 0; } //25
  wait_ms(300);
  training();
//  MsTimer2::set(5, chkAndCtl);
//  MsTimer2::start();
  tick.attach(&chkAndCtl, 0.005);
    myled = 0;
//    t.start();
} //30

void loop () { //31
wait_ms(10);
/*  pc1.printf("  thetaI:%d",thetaI);
  pc1.printf("  zeroOmegaI:%d",zeroOmegaI);
  pc1.printf("  omegaI:%d",omegaI);
  pc1.printf("  powerScale:%d",powerScale);
  pc1.printf("  power:%d\n",power);*/
//  if(t.read_ms() > 30)
  if(timer1 == 0)
  {
//  Serial.print("analogRead(A5):");Serial.print(analogRead(A5));
  pc.printf("%d,%d,%d,%d,%d\n",thetaI/5,omegaI*4,power*4,vE5/30,xE5/2);
//  t.reset();
   timer1 = 10;
  }
  if ( power > 0 ) {
    int power1 = min(power + 36, 255);
    pwmL = (float)power1 / 255.0f;
//    pwmL = 1.0f;
    fwdL = 1;
    revL = 0;
    pwmR = (float)power1 / 255.0f;
    fwdR = 1;
    revR = 0;
  } else {
    int power1 = max(power - 36, -255);
    pwmL = (float)-power1 / 255.0f;
    fwdL = 0;
    revL = 1;
    pwmR = (float)-power1 / 255.0f;
    fwdR = 0;
    revR = 1;
  }
} //47
void training(){ //48
  wait_ms (1000);
  for ( i = 0 ; i < 500 ; i++ ){ //50
//    zeroOmegaI = zeroOmegaI + (long)(ain.read() * 1023.0f);
    zeroOmegaI = zeroOmegaI + (long)(ain.read() * 675.18f);
  }
  zeroOmegaI = zeroOmegaI / i;
} //54
void chkAndCtl() { //55
 drct = 0;
 right = false;
 left = false;
 if(timer1 > 0) timer1--;
//  omegaI = (int)(ain.read() * 1023.0f) - zeroOmegaI;
  omegaI = (int)(ain.read() * 675.18f) - zeroOmegaI;
  if ( abs( omegaI ) < 5 ) { omegaI = 0; }
//  if ( abs( omegaI ) < 6 ) { omegaI = 0; }
  recOmegaI[0] = omegaI;
  thetaI = thetaI + omegaI;
  countS = 0; //60
  for ( i = 0 ; i < 10 ; i++ ) {
    if ( abs( recOmegaI[i] ) < 8 ) { countS++; }
//    if ( abs( recOmegaI[i] ) < 20 ) { countS++; }
  }
  if ( countS > 9 ) {
    thetaI = 0;// 65
    vE5 = 0;
    xE5 = 0;
    sumPower = 0;
    sumSumP = 0;
    flag = 1;
  } //70
  for ( i = 9 ; i > 0 ; i-- ) { recOmegaI[ i ] = recOmegaI[ i-1 ]; }
    powerScale = ( kAngle * thetaI / 200 ) + ( kOmega * omegaI / 78 ) + ( kSpeed * vE5 / 1000 ) + ( kDistance * xE5 / 1000 ); //72
    power = max ( min ( 95 * powerScale / 100 , 255 ) , -255 );
//    power = max ( min ( 95 * powerScale / 100 , 255 ) , -255 );
//    sumPower = sumPower + power + fwdBck * 4 - 21 ; // 74a
    sumPower = sumPower + power + fwdBck * 4 - 7 ; // 74a
//    sumPower = sumPower + power -6;
    sumSumP = sumSumP + sumPower;// 75
// vE5 = ??? //76
// xE5 = ??? //77
   vE5 = sumPower; //76a
   xE5 = sumSumP / 1000; //77a 
} //78
// Copyright (C) 2014 ArduinoDeXXX All Rights Reserved. //79